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Abstract Inverse dynamics is used extensively in ro¬ 
botics and biomechanics applications. In manipulator 
and legged robots, it can form the basis of an effective 
nonlinear control strategy by providing a robot with 
both accurate positional tracking and active compli¬ 
ance. In biomechanics applications, inverse dynamics 
control can approximately determine the net torques 
applied at anatomical joints that correspond to an ob¬ 
served motion. 

In the context of robot control, using inverse dynam¬ 
ics requires knowledge of all contact forces acting on the 
robot; accurately perceiving external forces applied to 
the robot requires filtering and thus significant time de¬ 
lay. An alternative approach has been suggested in re¬ 
cent literature: predicting contact and actuator forces 
simultaneously under the assumptions of rigid body dy¬ 
namics, rigid contact, and friction. Existing such inverse 
dynamics approaches have used approximations to the 
contact models, which permits use of fast numerical lin¬ 
ear algebra algorithms. In contrast, we describe inverse 
dynamics algorithms that are derived only from first 
principles and use established phenomenological mod¬ 
els like Coulomb friction. 

We assess these inverse dynamics algorithms in a 
control context using two virtual robots: a locomoting 
quadrupedal robot and a fixed-based manipulator grip¬ 
ping a box while using perfectly accurate sensor data 
from simulation. The data collected from these experi- 
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ments gives an upper bound on the performance of such 
controllers in situ. For points of comparison, we assess 
performance on the same tasks with both error feed¬ 
back control and inverse dynamics control with virtual 
contact force sensing. 

Keywords inverse dynamics, rigid contact, impact. 
Coulomb friction 


1 Introduction 


Inverse dynamics can be a highly effective method in 
multiple contexts including control of robots and phys¬ 
ically simulated characters and estimating muscle forces 
in biomechanics. The inverse dynamics problem, which 
entails computing actuator (or muscular) forces that 
yield desired accelerations, requires knowledge of all 
“external” forces acting on the robot, character, or hu¬ 
man. Measuring such forces is limited by the ability 
to instrument surfaces and filter force readings, and 
such filtering effectively delays force sensing to a degree 
unacceptable for real-time operation. An alternative is 
to use contact force predictions, for which reasonable 
agreement between models and reality have been ob¬ 
served (see, e.^., Aukes & Cutkosky 2013). Formulat¬ 


ing such approaches is technically challenging, however, 
because the actuator forces are generally coupled to the 
contact forces, requiring simultaneous solution. 

Inverse dynamics approaches that simultaneously 
compute contact forces exist in literature. Though these 
approaches were developed without incorporating all 
of the established modeling aspects (like complemen¬ 
tarity) and addressing all of the technical challenges 
(like inconsistent configurations) of hard contact, these 
methods have been demonstrated performing effectively 
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on real robots. In contrast, this article focuses on for¬ 
mulating inverse dynamics with these established model¬ 
ing aspects—which allows forward and inverse dynam¬ 
ics models to match—and addresses the technical chal¬ 
lenges, including solvability. 


1.1 Invertibility of the rigid contact model 


An obstacle to such a formulation has been the claim 


that the rigid contact model is not invertible (Tod- 


We will assess these inverse dynamics algorithms in 
the context of controlling a virtual locomoting robot 
and a fixed-base manipulator robot. We will examine 
performance of error feedback and inverse dynamics 
controllers with virtual contact force sensors for points 
of comparison. Performance will consider smoothness 
of torque commands, trajectory tracking accuracy, lo¬ 
comotion performance, and computation time. 


1.3 Contributions 


2^ , implying that inverse dynamics is unsolve- provides the following contributions: 


able for multi-rigid bodies subject to rigid contact. If 
forces on the multi-body other than contact forces at 
state { q,q } are designated x and contact forces are 
designated y, then the rigid contact model (to be de¬ 


scribed in detail in Section 2.3.3) yields the relationship 
y — there exists no left in¬ 

verse g{.) of / that provides the mapping x = gq^q{y) 
for y = fq^q{x). However, this article will show that 
there does exist a right inverse h{.) of / such that, 
for hq q{y) = X, fq^q{x) = y, and in Section we 
will show that this mapping is computable in expected 
polynomial time. This article will use this mapping to 
formulate inverse dynamics approaches for rigid con¬ 
tact with both no-slip constraints and frictional surface 
properties. 


1.2 Indeterminacy in the rigid contact model 

The rigid contact model is also known to be suscepti¬ 
ble to the problem of contact indeterminacy, the pres¬ 
ence of multiple equally valid solutions to the contact 
force-acceleration mapping. This indeterminacy is the 
factor that prevents strict invertibility and which, when 
used for contact force predictions in the context of in¬ 
verse dynamics, can result in torque chatter that is 
potentially destructive for physically situated robots. 
We show that computing a mapping from accelerations 
to contact forces that evolves without harmful torque 
chatter is no worse than NP-hard in the number of con¬ 
tacts modeled for Coulomb friction and can be calcu¬ 
lated in polynomial time for the case of infinite (no-slip) 
friction. 

This article also describes a computationally tract¬ 
able approach for mitigating torque chatter that is based 
upon a rigid contact model without complementarity 


conditions (see Sections 2.3.3 and 2.3.4). The model ap¬ 


pears to produce reasonable predictions: Anitescu (2006); 


Drumwright & Shell (2010); Todorov (2014) have all 


1. Proof that the coupled problem of computing in¬ 
verse dynamics-derived torques and contact forces 
under the rigid body dynamics, non-impacting rigid 
contact, and Coulomb friction models (with linearized 
friction cone) is solvable in expected polynomial time. 

2. An algorithm that computes inverse dynamics-derived 
torques without torque chatter under the rigid body 
dynamics model and the rigid contact model assum¬ 
ing no slip along the surfaces of contact, in expected 
polynomial time. 

3. An algorithm that yields inverse dynamics-derived 
torques without torque chatter under the rigid body 
dynamics model and the rigid, non-impacting con¬ 
tact model with Coulomb friction in exponential 
time in the number of points of contact, and hence a 
proof that this problem is no harder than NP-hard. 

4. An algorithm that computes inverse dynamics-derived 
torques without torque chatter under the rigid body 
dynamics model and a rigid contact model with 
Coulomb friction but does not enforce complemen¬ 


tarity conditions (again, see Sections 2.3.3 and 2.3.4), 
in expected polynomial time. 

These algorithms differ in their operating assump¬ 
tions. For example, the algorithms that enforce nor¬ 


used the model within simulation and physical artifacts 
have yet to become apparent. 


mal complementarity (to be described in Section 2.3) 
assume that all contacts are non-impacting; similarly, 
the algorithms that do not enforce complementarity as¬ 
sume that bodies are impacting at one or more points 
of contact. As will be explained in Section control 
loop period endpoint times do not necessarily coincide 
with contact event times, so a single algorithm must 
deal with both impacting and non-impacting contacts. 
It is an open problem of the effects of enforcing com¬ 
plementarity when it should not be enforced, or vice 
versa. The algorithms also possess various computa¬ 
tional strengths. As results of these open problems and 
varying computational strengths, we present multiple 
algorithms to the reader as well as a guide (see Ap¬ 
pendix]^ that details task use cases for these control¬ 
lers. 





































Inverse Dynamics with Rigid Contact and Friction 


3 


This work validates controllers based upon these ap¬ 
proaches in simulation to determine their performance 
under a variety of measurable phenomena that would 
not be accessible on present day physically situated 
robot hardware. Experiments indicating performance 
differentials on present day (prototype quality) hard¬ 
ware might occur due to control forces exciting unmod¬ 
eled effects, for example. Results derived from simula¬ 
tion using ideal sensing and perfect torque control in¬ 
dicate the limit of performance for control software de¬ 
scribed in this work. We also validate one of the more 
robust presented controllers on a fixed-base manipula¬ 
tor robot grasping task to demonstrate operation under 
disparate morphological and contact modeling assump¬ 
tions. 


1.4 Contents 


Section describes background in rigid body dynam¬ 
ics and rigid contact, as well as related work in inverse 
dynamics with contact and friction. We then present 
the implementation of three disparate inverse dynamics 
formulations in Sections iHl and[^ With each imple¬ 
mentation, we seek to: (i) successfully control a robot 
through its assigned task; ( 2 ) mitigate torque chatter 
from indeterminacy; ( 3 ) evenly distribute contact forces 
between active contacts; ( 4 ) speed computation so that 
the implementation can be run at realtime on stan¬ 
dard hardware. Section presents an inverse dynamics 
formulation with contact force prediction that utilizes 
the non-impacting rigid contact model (to be described 


in Section 2.3) with no-slip frictional constraints. Sec¬ 


tion presents an inverse dynamics formulation with 
contact force prediction that utilizes the non-impacting 
rigid contact model with Coulomb friction constraints. 
We show that the problem of mitigating torque chatter 
from indeterminate contact configurations is no harder 
than NP-hard. Section presents an inverse dynamics 
formulation that uses a rigid impact model and permits 
the contact force prediction problem to be convex. This 
convexity will allow us to mitigate torque chatter from 
indeterminacy. 

Section describes experimental setups for assess¬ 
ing the inverse dynamics formulations in the context of 
simulated robot control along multiple dimensions: ac¬ 
curacy of trajectory tracking; contact force prediction 
accuracy; general locomotion stability; and computa¬ 
tional speed. Tests are performed on terrains with var¬ 
ied friction and compliance. Presented controllers are 
compared against both PID control and inverse dynam¬ 
ics control with sensed contact and perfectly accurate 
virtual sensors. Assessment under both rigid and com¬ 
pliant contact models permits both exact and in-the- 


limit verification that controllers implementing these 
inverse dynamics approaches for control work as ex¬ 
pected. These experiments also examine behavior when 
modeling assumptions break down. Section analyzes 
the findings from these experiments. 


1.5 The multi-body 


This paper centers around a multi-body^ which is the 
system of rigid bodies to which inverse dynamics is 
applied. The multi-body may come into contact with 
“fixed” parts of the environment (e.^., solid ground) 
which are sufficiently modeled as non-moving bodies— 
this is often the case when simulating locomotion. Al¬ 
ternatively, the multi-body may contact other bodies, 
in which case effective inverse dynamics will require 
knowledge of those bodies’ kinematic and dynamic prop¬ 
erties — necessary for manipulation tasks. 

The articulated body approach can be extended to 
a multi-body to account for physically interacting with 
movable rigid bodies by appending the six degree-of- 
freedom velocities (vch) and external wrenches (/cb) of 
each contacted rigid body to the velocity and external 
force vectors and by augmenting the generalized inertia 
matrix (M) similarly: 


V = 


T + 

't^robot 't^cb 


lT 


fext [^robot ^cb ] 


M 


hdj-obot 0 

0 Mcb 


(1) 

( 2 ) 

( 3 ) 


Without loss of generality, our presentation will here¬ 
after consider only a single multi-body in contact with 
a static environment (excepting an example with a ma¬ 
nipulator arm grasping a box in Section . 


2 Background and related work 


This section surveys the independent parts that are 
combined to formulate algorithms for calculating in¬ 
verse dynamics torques with simultaneous contact force 


computation. Section [ 2 Tl discusses complementarity prob¬ 
lems, a domain outside the purview of typical roboti¬ 


cists. Section | 2 . 2 | introduces the rigid body dynamics 
model for Newtonian mechanics under generalized coor¬ 
dinates. Section [23] covers the rigid contact model, and 
unilaterally constrained contact. Sections |2.3.1| - [2.3.3| 
show how to formulate constraints on the rigid con¬ 
tact model to account for Coulomb friction and no-slip 


constraints. Section 2.3.4 describe an algebraic impact 
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model that will form the basis of one of the inverse dy¬ 
namics methods. Section [2^ describes the phenomenon 
of “indeterminacy” in the rigid contact model. Lastly, 
Sections 12.51 and 12.61 discusses other work relevant to 
inverse dynamics with simultaneous contact force com¬ 
putation. 


2.1 Complementarity problems 


Complementarity problems are a particular class of math¬ 
ematical programming problems often used to model 
hard and rigid contacts. A nonlinear complementarity 
problem (NCP) is composed of three nonlinear con¬ 
straints (Cottle et al. 1992), which taken together con¬ 
stitute a complementarity condition: 


x>0 
fix) > 0 
x^fix) = 0 


( 4 ) 

( 5 ) 

( 6 ) 


where x and f Henceforth, we will use 

the following shorthand to denote a complementarity 
constraint: 


0 < a A 6 > 0 (7) 

which signifies that a > 0, 6 > 0, and a • 6 = 0. 

A LCP, or linear complementarity problem (r, Q), 
where r G and Q G is the linear version of 

this problem: 

w = Qz + r 
w >0 
z>0 
Z^w = 0 


for unknowns z 

Theory of LCPs has been established to a greater 
extent than for NCPs. For example, theory has indi¬ 
cated certain classes of LCPs that are solvable, which 
includes both determining when a solution does not ex¬ 
ist and computing a solution, based on properties of the 
matrix Q (above). Such classes include positive definite 
matrices, positive semi-definite matrices, P-matrices, 


and Z-matrices, to name only a few; (Murty 1988 Cot¬ 


tle et ah, 1992) contain far more information on com¬ 


plementarity problems, including algorithms for solving 
them. Given that the knowledge of NCPs (including al¬ 
gorithms for solving them) is still relatively thin, this 
article will relax NCP instances that correspond to con¬ 
tacting bodies to LCPs using a common practice, lin¬ 
earizing the friction cone. 


Duality theory in optimization establishes a corre¬ 
spondence between LCPs and quadratic programs (seej^ot- 


tle et al. 1992 Pages 4 and 5) via the Karush-Kuhn- 
Tucker conditions; for example, positive semi-definite 
LCPs are equivalent to convex QPs. Algorithms for 
quadratic programs (QPs) can be used to solve LCPs, 
and vice versa. 


2.2 Rigid body dynamics 

The multi rigid body dynamics (generalized Newton) 
equation governing the dynamics of a robot undergoing 
contact can be written in its generalized form as: 


Mg = fc+ P^T + /ext 


(8) 


Equation introduces the variables /ext ^ (“ex- 

ternal”, non-actuated based, forces on the m degree- 
of-freedom multibody, like gravity and Coriolis forces); 
fc ^ (contact forces); unknown actuator torques 
T G {nq is the number of actuated degrees of 

freedom); and a binary selection matrix P G 
If all of the degrees-of-freedom of the system are ac¬ 
tuated P will be an identity matrix. For, e.^., legged 
robots, some variables in the system will correspond to 
unactuated, “floating base” degrees-of-freedom (DoF); 
the corresponding columns of the binary matrix P G 
]^(m-6)xm pg 2ero vcctors, while every other col¬ 
umn will possess a single “1”. 


2.3 Rigid contact model 


This section will summarize existing theory of modeling 
non-impacting rigid contact and draws from [Stewart fc 


Trinkle (1996); Trinkle et al. (1997); Anitescu & Potra 


(1997). Let us define a set of gap functions (j)i{x) (for i = 
1 ,..., g'), where gap function i gives the signed distance 
between a link of the robot and another rigid body (part 
of the environment, another link of the robot, an object 
to be manipulated, etc.) 


Our notation assumes independent coordinates x 
(and velocities v and accelerations i?), and that gen¬ 
eralized forces and inertias are also given in minimal 
coordinates. 
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Fig. 1 : The contact frame consisting of fi, s, and , t vectors cor¬ 
responding to the normal, first tangential, and second tangential 
directions (for 3D) to the contact surface. 


The gap functions return a positive real value if the 
bodies are separated, a negative real value if the bodies 
are geometrically intersecting, and zero if the bodies are 
in a “kissing” configuration. The rigid contact model 
specifies that bodies never overlap, i.e.: 

(I)i{x) > 0 for i = 1,..., g (9) 

One or more points of contact between bodies is deter¬ 
mined for two bodies in a kissing configuration {(j)i{x) = 
0). For clarity of presentation, we will assume that each 
gap function corresponds to exactly one point of contact 
(even for disjoint bodies), so that n = q. In the ab¬ 
sence of friction, the constraints on the gap functions 
are enforced by forces that act along the contact nor¬ 
mal. Projecting these forces along the contact normal 
yields scalars fjy ^, • • •, /at^ • The forces should be com¬ 
pressive {i.e., forces that can not pull bodies together), 
which is denoted by restricting the sign of these scalars 
to be non-negative: 

fNi >0 for i = 1 ,..., n ( 10 ) 

A complementarity constraint keeps frictional contacts 
from doing work: when the constraint is inactive {(fi > 0 ) 
no force is applied and when force is applied, the con¬ 
straint must be active {(fi = 0). This constraint is ex¬ 
pressed mathematically as fi • f^^ = 0. All three con¬ 
straints can be combined into one equation using the 
notation in Section [2Tl 

0 < fNi -k fi{x) >0 for i = 1 ,..., n ( 11 ) 


2.3.1 Modeling Coulomb friction 


Dry friction is often simulated using the Coulomb model, 
a relatively simple, empirically derived model that yields 
the approximate outcome of sophisticated physical in¬ 
teractions. Coulomb friction covers two regimes: stick- 
ing/rolling friction (where the tangent velocity at a 
point of contact is zero) and sliding friction (nonzero 
tangent velocity at a point of contact). Rolling friction 
is distinguished from sticking friction by whether the 
bodies are moving relative to one another other than 
at the point of contact. 

There are many phenomena Coulomb friction does 
not model, including “drilling friction” (it is straight¬ 
forward to augment computational models of Coulomb 
friction to incorporate this feature, as seen in Leine| 
& docker 2003), the Stribeck effect (Stribeck 1902), 


and viscous friction, among others. This article focuses 
only on Coulomb friction, because it captures impor¬ 
tant stick/slip transitions and uses only a single param¬ 
eter; the LuGRe model (Do et ah 2007), for example. 


is governed by seven parameters, making system iden¬ 
tification tedious. 

Coulomb friction uses a unitless friction coefficient, 
commonly denoted pt. If we define the tangent veloc¬ 
ities and accelerations in 3D frames (located at the 

point of contact) as vSilvTi and aSijaTi^ respec¬ 
tively, and the tangent forces as and /t^, then the 
sticking/rolling constraints which are applicable exactly 
when 0 = , can be expressed via the Fritz-John 


Trinkle et al. 1997): 


optimality conditions (Mangasarian & Fromovitz 1967 


0 < f^. — fsi - fri ^Si F a‘^. > 0 

fJ'ifmaSi + fSi f Og. + a\, = 0 
+ hi 


^T. = 0 


(14) 

(15) 

(16) 


These conditions ensure that the friction force lies within 


the friction cone (Equation 14) and that the friction 
forces push against the tangential acceleration (Equa¬ 
tions 


15-16) 


In the case of sliding at the contact (n^. 7 ^ 0 or 
TTi 7 ^ 0)5 the constraints become: 


These constraints can be differentiated with respect to 
time to yield velocity-level or acceleration-level con¬ 
straints suitable for expressing the differential algebraic 
equations (DAEs), as an index 1 DAE: 


hi fNi - 

- fl - ff >0 

(17) 

l^ifNiVSi + fSi\ 

=0 

(18) 

IJ-ifNiVTi + hiX 

M+4. = o 

(19) 


0 < fNi -L fi{^) ^0 if = 0 for i = 1,... ,n (12) 

0 < fNi -k fi{^) >0 if = 0 for i = 1,... ,n 

(13) 


Note that this case is only applicable if > 0, 

so there is no need to include such a constraint in Equa¬ 
tion (as was necessary in Equation . 
































6 


Samuel Zapolsky, Evan Drumwright 


The rigid contact model with Coulomb friction is 
subject to inconsistent configurations ( Stewart |^ 2QQQa), 
exemplified by Painleve’s Paradox ( Painlev^ 1895), in 
which impulsive forces may be necessary to satisfy all 
constraints of the model even when the bodies are not 
impacting. The acceleration-level dynamics can be ap¬ 
proximated using finite differences; a first order ap¬ 
proximation is often used (see, e.^., |Posa fc Tedrake[ 


2012), which moves the problem to the velocity/impul¬ 


sive force domain. Such an approach generally uses an 


algebraic collision law (see Chatterjee & Ruina 1998) 
to model all contacts, both impacting, as inelastic im¬ 
pacts; typical “time stepping methods” (Moreau 1983) 


for simulating dynamics often treat the generalized co¬ 
ordinates as constant over the small timespan of contac¬ 


t/impact (z.e., a first order approximation); see, e.g., Stew- 
[art fc Trinkle (2000). Stewart has shown that this ap¬ 
proximation converges to the solution of the continuous 


time formulation as the step size tends to zero (1998) 


Upon moving to the velocity/impulsive force do¬ 
main, Equations p!4}|T^ require a slight transformation 
to the equations: 


,2 n2 



( 20 ) 


( 21 ) 


( 22 ) 


and there is no longer separate consideration of stick- 
ing/rolling and slipping contacts. 


2.3.2 No-slip constraints 


If the Coulomb friction constraints are replaced by no- 
slip constraints, which is a popular assumption in leg¬ 
ged locomotion research, one must also use the dis¬ 
cretization approach; without permitting impulsive forces. 


slip can occur even with infinite friction (Lynch & Ma- 


1995[ ) . The no-slip constraints are then simply vs^ = 


= 0 (replacing Equations 20-22). 


2.3.3 Model for rigid, non-impacting contact with 
Coulomb friction 

The model of rigid contact with Coulomb friction for 
two bodies in non-impacting rigid contact at p can be 


summarized by the following equations: 


0 < /„ ± a„ > 0 


(23) 

0 < - 

ft -L /vf + vl > Q 

(24) 

0 = + fsy 

Ivl + vl 

(25) 

0 = 

vl + vf 

(26) 

0 = fifjids fsy 

Ul + of 

(27) 

0 ~ ^fn^t ftC 

'al + al 

(28) 


where /^, /g, and ft are the signed magnitudes of the 
contact force applied along the normal and two tan¬ 
gent directions, respectively; is the relative acceler¬ 
ation of the bodies normal to the contact surface; and 
Vs and Vt are the relative velocities of the bodies pro¬ 
jected along the two tangent directions. The operator 
T indicates that a • 6 = 0, for vectors a and b. De¬ 
tailed interpretation of these equations can be found 
in Trinkle et al. (1997); we present a summary below. 


Equation |23| ensures that (i) only compressive forces 
are applied (/^ > 0); (2) bodies cannot interpenetrate 
{on > 0); and (3) no work is done for frictionless con¬ 
tacts {fn ' On = 0). Equation 


24 


models Coulomb fric¬ 
tion: either the velocity in the contact tangent plane 
is zero—which allows frictional forces to lie within the 
friction cone—or the contact is slipping and the fric¬ 
tional forces must lie strictly on the edge of the fric¬ 
tion cone. Equations and [26]— applicable to sliding 
contacts (z.e., those for which 7^ 0 or 7^ 0)— 
constrain friction forces to act against the direction 
of slip, while Equations and ^ constrain frictional 
forces for rolling or sticking contacts {i.e., those for 
which Vs = Vt = 0) to act against the direction of tan¬ 
gential acceleration. 

These equations form a nonlinear complementarity 
problem (Cottle et al. 1992), and this problem may not 
possess a solution with nonimpulsive forces due to the 
existence of inconsistent configurations like Painleve’s 
Paradox (Stewart, 2QQQb| ). This issue led to the move¬ 
ment to the impulsive force/velocity domain for mod¬ 
eling rigid contact, which can provably model the dy¬ 
namics of such inconsistent configurations. 

A separate issue with the rigid contact model is that 
of indeterminacy, where multiple sets of contact forces 
and possibly multiple sets of accelerations—or veloci¬ 
ties, if an impulse-velocity model is used—can satisfy 
the contact model equations. Our inverse dynamics ap¬ 
proaches, which use rigid contact models, address in¬ 
consistency and, given some additional computation, 
can address indeterminacy (useful for controlled sys¬ 
tems). 
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2 . 3.4 Contacts without complementarity 


Complementarity along the surface normal arises from 
Equation for contacting rigid bodies that are not 
impacting. For impacting bodies, complementarity con¬ 


ditions are unrealistic (Chatterjee 1999). Though the 


distinction between impacting and non-impacting may 
be clear in free body diagrams and symbolic mathemat¬ 
ics, the distinction between the two modes is arbitrary 
in floating point arithmetic. This arbitrary distinction 
has led researchers in dynamic simulation, for exam¬ 
ple, to use one model—either with complementarity or 
without—for both impacting and non-impacting con¬ 
tact. 


Anitescu ( 2QQ6| ) described a contact model with¬ 


out complementarity (Equation 11) used for multi-rigid 


orov 


body simulation. Drumwright fc Sh'dl] ( |2Q1Q| ) and Tod- 


(2014) rediscovered this model (albeit with slight 
modifications, addition of viscous friction, and guar¬ 
antees of solution existence and non-negativity energy 


dissipation in the former); Drumwright & Shell (2010) 
also motivated acceptability of removing the comple¬ 
mentarity condition based on the work by [Chatterj^ 
(1999). When treated as a simultaneous impact model. 


the model is consistent with first principles. Addition¬ 


ally, using arguments in Smith et al. (2012), it can be 


shown that solutions of this model exhibit symmetry. 
This impact model, under the assumption of inelastic 
impact — it is possible to model partially or fully elas¬ 
tic impact as well, but one must then consider numer¬ 
ous models of restitution, see, e.^., [Chatterjee fc Ruina 
(1998) — will form the basis of the inverse dynamics ap¬ 


proach described in Section 


Complementarity-free impact model (single point 
of contact) 


dissipate kinetic energy maximally: 


• • • 1+ ■^Tv/r + 

minimize - t; M i; 


non—interpenetration: 


subject to: n t; > 0 


compressive normal forces 

fn>0 

Coulomb friction: 

fn > /s + /f 
first-order dynamics: 


(29) 

(30) 

(31) 

(32) 


v= v + M (n'/„+s'/s+t'/() 

(33) 


where /n, /s, and ft are the signed magnitudes of 
the impulsive forces applied along the normal and two 
tangent directions, respectively; v G and v G 
are the generalized velocities of the multi-body immedi¬ 
ately before and after impact, respectively; M G 
is the generalized inertia matrix of the m degree of free¬ 
dom multi-body; and n G s G and t G are 
generalized wrenches applied along the normal and two 
tangential directions at the point of contact (see Ap¬ 
pendix]^ for further details on these matrices). 

The physical interpretation of the impact model is 
straightforward: it selects impact forces that maximize 
the rate of kinetic energy dissipation. Finally, we note 
that rigid impact models do not enjoy the same de¬ 
gree of community consensus as the non-impacting rigid 
contact models because three types of impact mod¬ 
els (algebraic, incremental, and full deformation) cur¬ 
rently exist ( [Chatterjee fc Ruina 1998[ ), because simul¬ 
taneous impacts and impacts between multi-bodies can 


be highly sensitive to initial conditions (Ivanov 1995), 


and because intuitive physical parameters for captur¬ 
ing all points of the feasible impulse space do not yet 


exist (Chatterjee & Ruina 1998), among other issues. 


These difficulties lead this article to consider only in¬ 
elastic impacts, a case for which the feasible impulse 
space is constrained. 


2.4 Contact force indeterminacy 


The model is formulated as the convex quadratic 
program below. For consistency of presentation with 
the non-impacting rigid model described in the previous 
section, only a single impact point is considered. 


In previous work (Zapolsky et al., 2013), we found that 
indeterminacy in the rigid contact model can be a sig¬ 
nificant problem for controlling quadrupedal robots (and, 
presumably, hexapods, etc.) by yielding torques that 
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switch rapidly between various actuators (torque chat¬ 
ter). The problem can occur in bipedal walkers; for ex- 


Kuindersma et ah (2014) use a no-slip constraint but 


ample, Collins et ah (2001) observed instability from 


rigid contact indeterminacy in passive walkers. Even 
manipulators may also experience the phenomenon of 
rigid contact indeterminacy, indicated by torque chat¬ 
ter. 

Rigid contact configurations can be indeterminate 
in terms of forces; for the example of a table with all 
four legs perfectly touching a ground plane, infinite 
enumerations of force configurations satisfy the con¬ 


tact model (as discussed in Mirtich 1996), although 
the accelerations predicted by the model are unique. 
Other rigid contact configurations can be indetermi¬ 
nate in terms of predicting different accelerations/ve¬ 
locities through multiple sets of valid force configura¬ 
tions. We present two methods of mitigating indetermi¬ 


nacy in this article (see Sections 4.6 and 6.2.4). Defining 
a manner by which actuator torques evolve over time, 
or selecting a preferred distribution of contact forces 
may remedy the issues resulting from indeterminacy. 


2.5 Contact models for inverse dynamics in the 
context of robot control 

This section focuses on “hard”, by which we mean per¬ 
fectly rigid, models of contact incorporated into inverse 
dynamics and whole body control for robotics. We are 
unaware of research that has attempted to combine in¬ 
verse dynamics with compliant contact (one possible 
reason for absence of such work is that such compli¬ 
ant models can require significant parameter tuning 
for accuracy and to prevent prediction of large contact 
forces). 

[Mistry et al. (2010) developed a fast inverse dynam¬ 
ics control framework for legged robots in contact with 
rigid environments under the assumptions that feet do 


not slip. Righetti et al. (2013) extended this work with 


a framework that permits quickly optimizing a mixed 
linear/quadratic function of motor torques and contact 
forces using fast linear algebra techniques. [Hutter fc 


Siegwart (2012) also uses this formulation in an oper¬ 


ational space control scheme, simplifying the contact 
mathematics by assuming contacts are sticking. |Mistry| 
et al. I [Righetti et ^[Hutter fc Siegwart [ demonstrate ef¬ 
fective trajectory tracking performance on quadrupedal 
robots. 


The inverse dynamics approach of Ames (2013) as¬ 


sumes sticking impact upon contact with the ground 
and immediate switching of support to the new con¬ 
tact, while enforcing a unilateral constraint of the nor¬ 
mal forces and predicting no-slip frictional forces. 


allow for bounded violation of that constraint in order 
to avoid optimizing over an infeasible or inconsistent 
trajectory. 


Stephens & Atkeson (2010) incorporate a contact 


model into an inverse dynamics formulation for dy¬ 
namic balance force control. Their approach uses a quad¬ 
ratic program (QP) to estimate contact forces quickly 
on a simplified model of a bipedal robot’s dynamics. 


Newer work by Feng et al. (2013) builds on this by 


approximating the friction cone with a circumscribed 
friction pyramid. 


Ott et al. (2011) also use an optimization approach 


for balance, modeling contact to distribute forces among 
a set of pre-defined contacts to enact a generalized wrench 
on a simplified model of a biped; their controller seeks 
to minimize the Euclidian norm of the predicted contact 
forces to mitigate slip. In underconstrained cases (where 
multiple solutions to the inverse dynamics with contact 


system exist), Saab et al. (2013) and Zapolsky et al. 


( 2Q13[ ) use a multi-phase QP formulation for bipeds and 
quadrupeds, respectively. [Zapolsky et "S] mitigates the 
indeterminacy in the rigid contact model by selecting 
a solution that minimizes total actuator torques, while 


Saab et al. use the rigid contact model in the context 
of cascades of QPs to perform several tasks in paral¬ 
lel (z.e., whole body control). The latter work primar¬ 
ily considers contacts without slip, but does describe 
modifications that would incorporate Coulomb friction 
(inconsistent and indeterminate rigid contact configura¬ 
tions are not addressed). [Todorov ( |2Q14[ ) uses the same 
contact model (to be described below) but without us¬ 
ing a two-stage solution; that approach uses regular¬ 
ization to make the optimization problem strictly con¬ 
vex (yielding a single, globally optimal solution). None 
of |Saab et al.| [Zapolsky et al.| |Todorov| utilize the com¬ 
plementarity constraint (z.e., /at T 0 in Equation [IT]) in 
their formulation. Zapolsky et al and Todorov motivate 
dropping this constraint in favor of maximizing energy 
dissipation through contact, an assumption that they 


show performs reasonably in practice (Drumwright & 


Shell 2010 


Todorov 


2011 ) 


2.6 Contact models for inverse dynamics in the 
context of biomechanics 

Inverse dynamics is commonly applied in biomechan¬ 
ics to determine approximate net torques at anatomi¬ 
cal joints for observed motion capture and force plate 
data. Standard Newton-Euler inverse dynamics algo¬ 


rithms (as described in Featherstone 2008) are applied; 
least squares is required because the problem is over¬ 
constrained. Numerous such approaches are found in 
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biomechanics literature, including (Kuo| |I998| |Hatze 

to 

o 

o 

to 

Blajer et al. 2007 Bisseling & Hofl 20061 |Yang 

et al. 

|2007| |Van Den Bogert & Su, 20081 Sawers & 

Hahn 

|20I0|). These force plate based approaches nee- 


essarily limit the environments in which the inverse dy¬ 
namics computation can be conducted. 


3 Discretized inverse dynamics 


We discretize inverse dynamics because the resolution 
to rigid contact models both without slip and with 
Coulomb friction can require impulsive forces even when 
there are no impacts (see Section 2.3.1). This choice will 
imply that the dynamics are accurate to only first or¬ 
der, but that approximation should limit modeling error 


considerably for typical control loop rates (Zapolsky & 


Drumwright 2015). 


As noted above, dynamics are discretized using a 
first order approximation to acceleration. Thus, the so¬ 
lution to the equation of motion v = over [to, t/] 

is approximated by v = where At = 

{tf — to). We use the superscript to denote that 
a term is evaluated at ty and the superscript is 
applied to denote that a term is computed at to. For 
example, the generalized inertia matrix M has the su¬ 
perscript and the generalized post-contact velocity 
( v) has the superscript We will hereafter adopt 
the convention that application of a superscript once 
will indicate implicit evaluation of that quantity at that 
time thereafter (unless another superscript is applied). 
For example, we will continue to treat M as evaluated 
at to in the remainder of this paper. 


The remainder of this section describes how contact 
constraints should be determined for discretized inverse 
dynamics. 


3.1 Incorporating contact into planned motion 

First, we note that the inverse dynamics controller at¬ 
tempts to realize a planned motion. That planned mo¬ 
tion must account for pose data and geometric models 
of objects in the robot’s environment. If planned motion 
is inconsistent with contact constraints, e.g., the robot 
attempts to push through a wall, undesirable behavior 
will clearly result. Obtaining accurate geometric data 
(at least for novel objects) and pose data are presently 
challenging problems; additional work in inverse dy¬ 
namics control with predictive contact is necessary to 
address contact compliance and sensing uncertainty. 


3.2 Incorporating contact constraints that do not 
coincide with control loop period endpoint times 

Contact events—making or breaking contact, transi¬ 
tioning from sticking to sliding or vice versa—do not 
generally coincide with control loop period endpoint 
times. Introducing a contact constraint “early”, i.e., be¬ 
fore the robot comes into contact with an object, will 
result in a poor estimate of the load on the robot (as 
the anticipated link-object reaction force will be ab¬ 
sent). Introducing a contact constraint “late”, i.e., af¬ 
ter the robot has already contacted the object, implies 
that an impact occurred; it is also likely that actuators 
attached to the contacted link and on up the kinematic 
chain are heavily loaded, resulting in possible damage 
to the robot, the environment, or both. Figure [^depicts 
both of these scenarios for a walking bipedal robot. 




Fig. 2: If the contact constraint is introduced early (left figure, 
constraint depicted using dotted line) the anticipated load will be 
wrong. The biped will pitch forward, possibly falling over in this 
scenario. If the contact constraint is introduced late, an impact 
may occur while the actuators are loaded. The biped on the right 
is moving its right lower leg toward a foot placement; the im¬ 
pact as the foot touches down is prone to damaging the loaded 
powertrain. 


We address this problem by borrowing a constraint 


stabilization (Ascher et al. 1995) approach from An- 


itescu & Hart (2004), which is itself a form of Baum- 


garte Stabilization (Baumgarte 1972). Recalling that 
two bodies are separated by signed distance 0(.), con¬ 
straints on velocities are determined such that . 

To realize these constraints mathematically, we first 
convert Equation \T2\ to a discretized form: 


0 < fNi{t) T (j)i{x{t + At)) > 0 if (l)i{t) = 0 
for i = I,...,n 


(34) 


This equation specifies that a force is to be found such 
that applying the force between one of the robot’s links 
and an object, already in contact at t, over the interval 
[t, t + At] yields a relative velocity indicating sustained 
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contact or separation at t ^ At. We next incorporate 
the signed distance between the bodies: 

0 < /nM ^ + At)) > (35) 

for i = 1 ,..., n 


The removal of the conditional makes the constraint al¬ 
ways active. Introducing a constraint of this form means 
that forces may be applied in some scenarios when they 
should not be (see Figure]^ for an example). Alterna¬ 
tively, constraints introduced before bodies contact can 
be contradictory, making the problem infeasible. Exist¬ 
ing proofs for time stepping simulation approaches in¬ 
dicate that such issues disappear for sufficiently small 
integration steps (or, in the context of inverse dynamics. 


sufficiently high frequency control loops); see Anitescu 


& Hart (2004), which proves that such errors are uni¬ 


formly bounded in terms of the size of the time step 
and the current value of the velocity. 




Fig. 3: An example of a contaet eonstraint that overly constrains 
the motion between two disjoint bodies (the right foot and the 
skateboard). The contaet constraint will keep the foot from mov¬ 
ing below the dotted line. However, if the foot moves quickly 
downward and the skateboard moves quickly to the right (in At 
time), inverse dynamics may predict, incorrectly, that a contact 
force will be applied to the foot. It should be apparent that these 
problems disappear as At —)■ 0; i.e., as the control loop frequency 
becomes sufficiently high. 


3.3 Computing points of contact between geometries 

Given object poses data and geometric models, points 
of contact between robot links and environment objects 
can be computed using closest features. The partic¬ 
ular algorithm used for computing closest features is 
dependent upon both the representation (e.g., implicit 
surface, polyhedron, constructive solid geometry) and 
the shape (e.g., sphere, cylinder, box). Algorithms and 


code can be found in sources like Ericson (2005) and 
http: //geometrictools. com, E igurej^depicts the pro¬ 
cedure for determining contact points and normals for 
two examples: a sphere vs. a half-space and for a sphere 
vs. a sphere. 

Eor disjoint bodies like those depicted in Eigure|^ 
the contact point can be placed anywhere along the 
line segment connecting the closest features on the two 
bodies. Although the illustration depicts the contact 
point as placed at the midpoint of this line segment, 
this selection is arbitrary. Whether the contact point 
is located on the first body, on the second body, or 
midway between the two bodies, no formula is “correct” 
while the bodies are separated and every formula yields 
the same result when the bodies are touching. 



Fig. 4: Depiction of the manner of selecting points of contact 
and surface normals for disjoint bodies with spherical/half-space 
(left) and spherical/spherical geometries (right). Closest points 
on the objects are connected by dotted line segments. Surface 
normals are depicted with an arrow. Contact points are drawn 
as white circles with black outlines. 


4 Inverse dynamics with no-slip constraints 


Some contexts where inverse dynamics may be used 
(biomechanics, legged locomotion) may assume absence 


of slip (see, e.^., Righetti, et al.[ 2011 Zhao, et al. 


2Q14[ ). This section describes an inverse dynamics ap¬ 
proach that computes reaction forces from contact us¬ 
ing the non-impacting rigid contact model with no-slip 
constraints. Using no-slip constraints results in a sym¬ 
metric, positive semidefinite LCP. Such problems are 
equivalent to convex QPs by duality theory in optimiza¬ 


tion (see Cottle et al. 1992), which implies polynomial 
time solvability. Convexity also permits mitigating in¬ 
determinate contact configurations, as will be seen in 
Section [TG) This formulation inverts the rigid contact 
problem in a practical sense and is derived from first 
principles. 

We present two algorithms in this section: Algo¬ 
rithm ensures the no-slip constraints on the contact 
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model are non-singular and thus guarantees that the in¬ 
verse dynamics problem with contact is invertible; Al¬ 
gorithm [^presents a method of mitigating torque chat¬ 
ter from indeterminate contact (for contexts of inverse 


dynamics based control) by warm-starting (Nocedal & 


Wright 2006) the solution for the LCP solver with the 


last solution. 


4.1 Normal contact constraints 


shown that for sufficiently small Z\t, v converges to 
the solution of the continuous time dynamics and con¬ 
tact equations (1998). 


4.3 Inverse dynamics constraint 

The inverse dynamics constraint is used to specify the 
desired velocities only at actuated joints: 

Pv = qdes (38) 


The equation below extends Equation to multiple 
points of contact (via the relationship 0 = Ni;), where 
N G is the matrix of generalized wrenches along the 
contact normals (see Appendix [b|) : 


d) - + 

< N v T /at > 0 

At - - 


(36) 


Because 0 is clearly time-dependent and the control 
loop executes at discrete points in time, N must be 
treated as constant over a time interval. N indicates 
that points of contact are drawn from the current con¬ 
figuration of the environment and multi-body. Analo¬ 
gous to time-stepping approaches for rigid body simu¬ 
lations with rigid contact, all possible points of contact 
between rigid bodies over the interval to and tf can be 
incorporated into N as well: as in time stepping ap¬ 
proaches for simulation, it may not be necessary to ap¬ 
ply forces at all of these points (the approaches im¬ 
plicitly can treat unnecessary points of contact as inac¬ 
tive, though additional computation will be necessary). 


Stewart (1998) showed that such an approach will con¬ 


verge to the solution of the continuous time dynamics 
as At = {tf — to) 0. Given a sufficiently high con¬ 
trol rate. At will be small and errors from assuming 
constant N over this interval should become negligible. 


4.2 Discretized rigid body dynamics equation 


The discretized version of Equation now separating 
contact forces into normal ( N) and tangential wrenches 
( S and T are matrices of generalized wrenches along 
the first and second contact tangent directions for all 
contacts) is: 

M( % - ~v) =NT/jv + S^fs + T^fr - • • • (37) 

P'^T + At /ext 


Treating inverse dynamics at the velocity level is nec¬ 
essary to avoid the inconsistent configurations that can 
arise in the rigid contact model when forces are re¬ 
quired to be non-impulsive (Stewart 2000a as also 
noted in Section 2.3.1). As noted above, Stewart has 


Desired velocities ^des are calculated as: 
^des = ^ T 


4.4 No-slip (infinite friction) constraints 


Utilizing the first-order discretization (revisit Section 2.3.2 
to see why this is necessary), preventing tangential slip 
at a contact is accomplished by using the constraints: 


S i; = 0 
T^v = 0 


(40) 

(41) 


These constraints indicate that the velocity in the tan¬ 
gent plane is zero at time tf] we will find the matrix 
representation to be more convenient for expression as 
quadratic programs and linear complementarity prob¬ 
lems than: 

Vsi^= 0 for i = 1 ,... ,n. 


z.e., the notation used in Section [2.3.1 
equations are compiled below: 


All presented 


Complementarity-based inverse dynamics without 
slip 


non—interpenetration, compressive force, and 
normal complementarity constraints: 

0 < /jv -L > —$■ 

-JIV _ 

no—slip constraints: 

= 0 

T^v = 0 

inverse dynamics: 

= 9des 

first-order dynamics: 

^v=~v + M-\N'^fN + S'^fs + ... 
T'^fr - + Atfe^t) 
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Combining EquatiQns [36H38l[4Ql and [IT] into a mixed 

CP, see Appendix[C|) 


linear eomplementarity pro 
yields: 


-M -pT -ST -TT -NT- 


r + 1 

V 


K 


- 0 - 

P 0 0 0 0 

S 0 0 0 0 


T 

fs 

+ 

Qdes 

0 

— 

0 

0 

T 0 0 0 0 


fr 


0 


0 

_N 0 0 0 0 _ 


Jn_ 


0 

^ At 


wn- 


fN > 0 , '^N > 0 , fJjWN = 0 


(42) 


(43) 


where k, = —Z\t/ext v. We define the MLCP block 

matrices—in the form of Equations |116H119| from Ap- 
pendix[C]— and draw from Equations |4^ and [43| to yield: 


A = 


D = -C^ 


X = 


y = fN 


"M 

-pT 

-s^ 

_TT- 



p 

0 

0 

0 


0 

S 

0 

0 

0 

c = 

0 

T 

0 

0 

0 


0 


B = 0 


r+ n 

V 


— K 

T 

9 = 

^des 

fs 

0 

Jt_ 


0 


121 


h = ^ 

At 

and |122| (again see Ap- 


Applying Equations 

pendix [^, we transform the MLCP to LCP (r,Q). 
Substituting in variables from the no-slip inverse dy¬ 
namics model and then simplifying yields: 


Q = C'A-^C 

r=-^ + C'^A-^ 

At 


(44) 

(45) 


The definition of matrix A from above may be singular, 
which would prevent inversion, and thereby, conversion 
from the MLCP to an LCP. We defined P as a selection 
matrix with full row rank, and the generalized inertia 
(M) is symmetric, positive definite. If S and T have full 
row rank as well, or we identify the largest subset of row 
blocks of S and T such that full row rank is attained, A 
will be invertible as well (this can be seen by applying 
blockwise matrix inversion identities). Algorithm [^per¬ 
forms the task of ensuring that matrix A is invertible. 
Removing the linearly dependent constraints from the 
A matrix does not affect the solubility of the MLCP, 
as proved in Appendix E 


Erom Bhatia ( 2QQ7[ ), a matrix of Q’s form must be 
non-negative definite, z.e., either positive-semidefinite 
(PSD) or positive definite (PD). Q is the right product 
of C with its transpose about a symmetric PD matrix, 
A. Therefore, Q is symmetric and either PSD or PD. 


The singularity check on Lines i and of Algo¬ 
rithm is most quickly performed using Cholesky fac¬ 
torization; if the factorization is successful, the matrix 
is non-singular. Given that M is non-singular (it is sym¬ 
metric, PD), the maximum size of X in Algorithm [^ is 
m X m; if X were larger, it would be singular. 

The result is that the time complexity of Algorithmic 
is dominated by Lines and As X changes by at 
most one row and one column per Cholesky factoriza¬ 
tion, singularity can be checked by 0 {m?) updates to an 
initial 0{m^) Cholesky factorization. The overall time 
complexity is 0 {m^ -h nim?). 


Algorithm 1 Eind-Indices(M, P, S, T), determines 
the row indices (5, and T) of S and T such that the ma¬ 
trix A (Equation |116| in Appendix E is non-singular. 


1 : 5<-0 

2 : r ^0 

3: for i = 1,..., n do 0 n is the number of contacts 

4: 

5: Set X ^ [PT Sj, ] 

6: if X^M“^X not singular then 

7: 5^5* 

8: r*^rup} 

9: Set X ^ [PT Sj tT,] 

10: if X^M“^X not singular then 

11 : r^r* 

12: return {*S,T} 


4.5 Retrieving the inverse dynamics forces 


Once the contact forces have been determined, one solves 


Equations 


37 


and 


38 


for { V, r}, thereby obtaining the 


inverse dynamics forces. While the LCP is solvable, it 
is possible that the desired accelerations are inconsis¬ 
tent. As an example, consider a legged robot standing 
on a ground plane without slip (such a case is similar 
to, but not identical to infinite friction, as noted in Sec¬ 


tion 2.3.2), and attempting to splay its legs outward 


while remaining in contact with the ground. Such cases 

can be readily identified by verifying that ^ • 

If this constraint is not met, consistent desired accelera¬ 
tions can be determined without resolving the LCP. Eor 
example, one could determine accelerations that devi¬ 
ate minimally from the desired accelerations by solving 
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a quadratic program: 


minimize 

WP^v - qdesll 

(46) 

subject to: 

IV 

(47) 


S v = 0 

(48) 


Xt; = 0 

(49) 


= M“t; + N'T/jv + S'^fs + ... 

(50) 


T'^fr + P'^T + AtUt 

(51) 


This QP is always feasible: r = 0 ensures that 
St; = 0, and xt; = 0. 


Using warm starting, Algorithm will find a solu- 
tion that predicts contact forces applied at the exact 
same points on the last iteration assuming that such a 
solution exists. Such solutions do not exist (i) when the 
numbers and relative samples from the contact mani¬ 
fold change or ( 2 ) as contacts transition from active 
{(j)i{x^v) < 0 ) to inactive {(j)i{x^v) > 0 ), or vice versa. 
Case ( 2 ) implies immediate precedence or subsequence 
of case ( 1 ), which means that discontinuities in actu¬ 
ator torques will occur for at most two control loop 
iterations around a contact change (one discontinuity 
will generally occur due to the contact change itself). 

4.7 Scaling inverse dynamics runtime linearly in 
number of contacts 


4.6 Indeterminacy mitigation 


We warm start a pivoting LCP solver (see Appendix [D|) 
to bias the solver toward applying forces at the same 
points of contact (see Figure —tracking points of con¬ 
tact using the rigid body equations of motion — as were 
active on the previous inverse dynamics call (see Al¬ 
gorithm [^. [Knindersma et ^ (2014) also use warm 
starting to solve a different QP resulting from contact 
force prediction. However, we use warm starting to ad¬ 
dress indeterminacy in the rigid contact model while 


Knindersma et ah] use it to generally speed the solution 


process. 


Fig. 5: Warm-Starting Example 


Iteration i Iteration i + 1 Iteration i + 2 



Left (“cold start”): with four active contacts, the 
pivoting solver chooses three arbitrary non-basic 
indices (in see Appendix]^ to solve the LCP and 
then returns the solution. The solution applies the 
majority of upward force to two feet and applies a 
smaller amount of force to the third. 

Center (“warm start”): With four active contacts, 
the pivoting solver chooses the same three non-basic 
indices as the last solution to attempt to solve the 
LCP. The warm-started solution will distribute 
upward forces similarly to the last solution, tending 
to provide consecutive solves with continuity over 
time. 

Right (“cold start”): one foot of the robot has 
broken contact with the ground; there are now three 
active contacts. The solver returns a new solution, 
applying the majority of normal force to two legs, 
and applying a small amount of force to the third. 


The multi-body’s number of generalized coordinates (m) 
are expected to remain constant. The number of contact 
points, n, depends on the multi-body’s link geometries, 
the environment, and whether the inverse dynamics ap¬ 
proach should anticipate potential contacts in [to^tf] 
(as discussed in Section 4.1). This section describes a 
method to solve inverse dynamics problems with simul¬ 
taneous contact force computation that scales linearly 
with additional contacts. This method will be applica¬ 
ble to all inverse dynamics approaches presented in this 
paper except that described in Section that problem 
results in a copositive LCP (Cottle et al. 1992) that 
the algorithm we describe cannot generally solve. 

To this point in the article presentation, time com¬ 
plexity has been dominated by the O(n^) expected time 
solution to the LCPs. However, a system with m degrees- 
of-freedom requires no more than m positive force mag¬ 
nitudes applied along the contact normals to satisfy 
the constraints for the no-slip contact model. Proof is 
provided in Appendix [F] Below, we describe how that 
proof can be leveraged to generally decrease the ex¬ 
pected time complexity. 


Modified PPM I Algorithm We now describe a modifi¬ 


cation to the Principal Pivoting Method I (Cottle et al. 


|1992[ ) (PPM) for solving LCPs (see description of this 
algorithm in Appendix that leverages the proof in 


Appendix [ f| to attain expected 0(r 


Q time ( 


plexity. A brief explanation of the mechanics of pivoting 
algorithms is provided in Appendix we use the com¬ 
mon notation of /3 as the set of basic variables and as 
the set of non-basic variables. 

The PPM requires few modifications toward our 
purpose. These modifications are presented in Algo¬ 
rithmic First, the full matrix NM“^N^ is never con¬ 
structed, because the construction is unnecessary and 
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would require 0{n^) time. Instead, Line 11 of the al¬ 
gorithm constructs a maximum m x m system; thus, 
that operation requires only 0{m^) operations. Sim¬ 
ilarly, Lines 12 and 13 also leverage the proof from 
Appendixto compute and efficiently (though 
these operations do not affect the asymptotic time com¬ 
plexity). Expecting that the number of iterations for a 


pivoting algorithm is 0{n) in the size of the input (Cot 


tie et al. 1992) and assuming that each iteration re¬ 


quires at most two pivot operations (each rank-1 up¬ 
date operation to a matrix factorization will exhibit 
0 {rin?) time complexity), the asymptotic complexity of 
the modified PPM I algorithm is 0{m^ -\-rin?n). The ter¬ 
mination conditions for the algorithm are not affected 
by our modifications. 


Algorithm 2 {z,w,B} = PPM(Ar, M, /*, z ) 
Solves the LCP (NM-^*, NM-^N"^) resulting 
from convex, rigid contact models (the no-slip model 
and the complementarity-free model with Coulomb 
friction) . B are the set of non-basic indices re¬ 
amed from the last call to PPM. 

1 : 

2 : 

3: 

4: 

5: 

6 : 

7: 


> Check for trivial solution 


9: 

10 : 

11 : 

12 : 

13: 

14: 

15: 

16: 

17: 

18: 

19: 

20 

21 

22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 


n <r- rows(N) 

r ^ N • /* 
i ^ arg min^ ri 
if rj > 0 then 
return {0,r} 

B = ^ then 
B^{i} 

while true do 
A e 

- A-i • -h 

^ +/* 

t ^ N • at 

i ^ arg min^ w\ > Search for index to move into 

non-basic set 


> Establish initial nonbasic indices 
> Establish basic indices 


h< 

; 2 t 

at 

w 


■ N^- M-i 




> Compute z non-basic components 


nt 


.t 


j ^ argmm^ z'^ > No index to move into the 

non-basic set; search for index to move into the basic set 

if zj < 0 then 

m 

> Move index k into basic set 


k ■ 

B^BU{k} 

B^B-{k} 

continue 

else 

z ^ 0 
z-i^ ^ 2:t 


^B 

w ^ 0 


fd 


Ws ^ w' 
return {z, w} 


else 
B- 
B ■ 


BU{i} 

B-{i} 


> Move index i into non-basic set 


vt 


j ^ arg mm. 
into the basic set 

if 2 ;j < 0 then 
k ^ B{j) 
B^BU{k} 
B^B-{k} 


0 Try to hnd an index to move 


> Move index k into basic set 


5 Inverse dynamics with Coulomb friction 


Finally, we note that Baraff (1994) has proven that 
LCPs of the form (Hio, HQ-m' ), where H G W^i, Q G 
]^gxg,w ^ Q^j^d Q is symmetric, PD are solvable. 
Thus, the inverse dynamics model will always possess 
a solution. 


Though control with the absence of slip may facili¬ 
tate grip and traction, the assumption is often not the 
case in reality. Foot and manipulator geometries, and 
planned trajectories must be specially planned to pre¬ 
vent sliding contact, and assuming sticking contact may 
lead to disastrous results (see discussion on experimen¬ 


tal results in Section 8.2). Implementations of control¬ 


lers that limit actuator forces to keep contact forces 
within the bounds of a friction constraint have been 
suggested to reduce the occurrence of unintentional slip 
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in walking robots (Righetti et al. 2013). These meth¬ 
ods also limit the reachable space of accelerative tra¬ 
jectories that the robot may follow, as all movements 
yielding sliding contact would be infeasible. The model 
we present in this section permits sliding contact. We 
formulate inverse dynamics using the computational 
model of rigid contact with Coulomb friction devel¬ 


oped by Stewart & Trinkle (1996) and Anitescu & Potra 


(1997); the equations in this section closely follow those 


m 


Anitescu & Potra (1997). 


5.1 Coulomb friction constraints 


Still utilizing the first-order discretization of the rigid 


body dynamics (Equation 37), we reproduce the lin¬ 
earized Coulomb friction constraints from lAnitescn fcl 


Potra (1997) without explanation (identical except for 
slight notational differences): 


0 < EA + F V _L /f > 0 (52) 

0 < M/iv - E^/f ± a > 0 (53) 


where E G (k is the number of edges in the 

polygonal approximation to the friction cone) retains 
its definition from Anitescu & Potra (1997) as a sparse 
selection matrix containing blocks of “ones” vectors, 
/jL G is a diagonal matrix with elements corre¬ 

sponding to the coefficients of friction at the n con¬ 
tacts, A is a variable roughly equivalent to magnitude 
of tangent velocities after contact forces are applied 
and F G (equivalent to D in I Anitescu fc Potra| 


1997) is the matrix of wrenches of frictional forces at 


k tangents to each contact point. If the friction cone is 
approximated by a pyramid (an assumption we make 
in the remainder of the article), then: 


F = -S^ 


/f 


p+T P-T P+T p —T 

JS JS JT Jt 


where fs = fs ~ fs = fr ~ fr • Given these 

substitutions, the contact model with inverse dynamics 
becomes: 



5.2 Resulting MLCP 


Combining Equations |36l-[38| and [52]-[53l results in the 
MLCP: 


-M -pT -NT -pT 0- 

P 0 0 0 0 

NO 0 0 0 

F 0 0 0 E 

_0 0 -ET 0_ 

fN > 0 , Wn > 0 , fJjWN - 

fp > 0 , wp > 0 , fpWp = 

A > 0, wx > 0, X^wx = 0 


” + 

V 


— K, 


■ 0 - 

T 


Qdes 


0 

fN 

fp 

+ 

0 

At 

0 

— 

Wn 

Wp 

A 


0 


-'Wx- 


^ 0 
0 


(59) 


(60) 

(61) 

(62) 


Vectors and Wp correspond to the normal and tan¬ 
gential velocities after impulsive forces have been ap¬ 
plied. 


5.2.1 Transformation to TCP and proof of solution 
existence 


The MLCP can be transformed to a LCP as described 


by Cottle et al. (1992) by solving for the unconstrained 

- ^ -- 


variables v and r. This transformation is possible be¬ 
cause the matrix: 


X = 


M PT 
P 0 


(63) 


is non-singular. Proof comes from blockwise invertibil- 
ity of this matrix, which requires only invertibility of 
M (guaranteed because generalized inertia matrices are 
positive definite) and PM“^P^. This latter matrix se¬ 
lects exactly those rows and columns corresponding to 


the joint space inertia matrix (Eeatherstone 1987), which 
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is also positive definite. After eliminating the uncon¬ 
strained variables v and r, the following LCP results: 




fN 



['^N 1 

FM-^f’’’ 0 


f F 

+ 



-e''^ 0 . 


X 


-FM“^k^ 

L 0 J 

L J 


/v > 0, '^N > 0, fJjWN = 0 


(64) 

(65) 


fp > 0 , wf > 0 , fpWF = 0 


( 66 ) 


A > 0, w\ > 0, w\ = 0 


(67) 


The discussion in Stewart & Trinkle (1996) can be 


used to show that this LCP matrix is copositive (see Cot- 


tie, et si] 1992[ Definition 3.8.1), since for any vector 


^ — [fN^ A"*"] > 0 , 


fN 

T 

NM-^NT NM-ipT E' 


fN 

fp 


FM-^N^ FM-^F^ 0 


fp 

X 


-E'r 0 


X 


(NT/^ + fT/^)^M- 1(NT/^ + pT/^) + > 0 

( 68 ) 


because is positive definite and fi is a diagonal 
matrix with non-negative elements. The transformation 
from the MLCP to the LCP yields {k + 2)n LCP vari¬ 
ables (the per-contact allocation is: one for the normal 
contact magnitude, k for the frictional force compo¬ 
nents, and one for an element of A) and at most 2 m 
unconstrained variables. 

As noted by Anitescu & Potra (1997), Lemke’s Algo¬ 


rithm can provably solve such copositive LCPs (Cottle 


et al. 1992) if precautions are taken to prevent cycling 


through indices. After solving the LCP, joint torques 
can be retrieved exactly as in Section |4.5| Thus, we 
have shown—using elementary extensions to the work 
Anitescu & Potr^ {lOOl ) —that a right inverse of 


the non-impacting rigid contact model exists (as first 
broached in Section [^. Additionally, the expected run¬ 
ning time of Lemke’s Algorithm is cubic in the number 
of variables, so this inverse can be computed in expected 
polynomial time. 


5.3 Contact indeterminacy 

Though the approach presented in this section produces 
a solution to the inverse dynamics problem with simul¬ 
taneous contact force computation, the approach can 
converge to a vastly different, but equally valid solution 
at each controller iteration. However, unlike the no-slip 
model, it is unclear how to bias the solution to the LCP, 
because a means for warm starting Lemke’s Algorithm 
is currently unknown (our experience confirms the com¬ 
mon wisdom that using the basis corresponding to the 
last solution usually leads to excessive pivoting). 


Generating a torque profile that would evolve with¬ 
out generating torque chatter requires checking all pos¬ 
sible solutions of the LCP if this approach is to be used 
for robot control. Generating all solutions requires a lin¬ 
ear system solve for each combination of basic and non- 
basic indices among all problem variables. Enumerating 
all possible solutions yields exponential time complex¬ 
ity, the same as the worst case complexity of Lemke’s 
Algorithm (Lemke 1965). After all solutions have been 
enumerated, torque chatter would be eliminated by us¬ 
ing the solution that differs minimally from the last 
solution. Algorithm presents this approach. 


Algorithm 3 {x, e} =MINDIFF(A, B, C, D, g, h, B, Xq, 

Computes the solution to the LCP 

that is closest (by 
Euclidean norm) to vector xq using a recursive 
approach, n is always initialized as rows(B). 

1: if n > 0 then 

2: =MINDIFF(A,B,C,D,£/,h,:B,®o,n- 1) 

3: Bp- {B, n} > Establish nonbasic indices 

4: {* 2 , 62 } =MINDIFF(A,B,C,D,£/,h,:B,®o,n- 1) 

5: if ei < 62 then return {£Ci,ei} 

6: else return {* 2 , £ 2 } 

7: else 

8: {z, w} = LCP{hnb - B^^ - A“^C^b) 

9. X i A (f^nb^nb T dd) 

10: return {tc, 11® — £Co 11} 


The fact that we can enumerate all solutions to the 
problem in exponential time proves that solving the 
problem is at worst NP-hard, though following an enu- 
merative approach is not practical. 


6 Convex inverse dynamics without normal 
complementarity 

This section describes an approach for inverse dynam¬ 
ics that mitigates indeterminacy in rigid contact using 
the impact model described in Section |2.3.4[ The ap¬ 
proach is almost identical to the “standard” rigid con¬ 
tact model described in Section [231 but for the absence 
of the normal complementarity constraint. 

The approach works by determining contact and ac¬ 
tual forces in a first step and then solving within the 
nullspace of the objective function (Equation such 
that joint forces are minimized. The resulting problem 
is strictly convex, and thus torques are continuous in 
time (and more likely safe for a robot to apply) if the 
underlying dynamics are smooth. This latter assump¬ 
tion is violated only when a discontinuity occurs from 
one control loop iteration to the next, as it would when 
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contact is broken, bodies newly come into contact, the 
contact surface changes, or contact between two bodies 
switches between slipping and sticking. 


Torque chatter due to contact indeterminacy can 
be avoided by ensuring that contact forces are not cy¬ 
cled rapidly between points of contact under potentially 
indeterminate contact configurations across successive 
controller iterations. Zapolsky & Drumwright (2014) 
eliminate torque chatter using a QP stage that searches 
over the optimal set of contact forces (using a convex 
relaxation to the rigid contact model) for forces that 
minimize the ^ 2 -norm of joint torques. 


6.1 Two-stage vs. one-stage approaches 


An alternative, one-stage approach is described by |Tod-| 
(2014), who regularizes the quadratic objective 


matrix to attain the requisite strict convexity. Another 
one-stage approach (which we test in Section uses 
the warm starting-based solution technique described 
in Section to mitigate contact indeterminacy. The 
two-stage approach described below confers the follow¬ 
ing advantages over one-stage approaches: (i) no reg¬ 
ularization factor need be chosen—there has yet to be 
a physical interpretation behind regularization factors, 
and computing a minimal regularization factor would 
be computationally expensive; and ( 2 ) the two-stage 
approach allows the particular solution to be selected 
using an arbitrary objective criterion—minimizing ac¬ 
tuator torques is particularly relevant for robotics ap¬ 
plications. Two stage approaches are somewhat slower, 
though we have demonstrated performance suitably fast 
for real-time control loops on quadrupedal robots in 


Zapolsky & Drumwright (2014). We present the two 


stage approach without further comment, as the reader 
can realize the one stage approach, if desired, by regu¬ 
larizing the Hessian matrix in the quadratic program. 


6.2 Computing inverse dynamics and contact forces 
simultaneously (Stage I) 

For simplicity of presentation, we will assume that the 
number of edges in the approximation of the friction 
cone for each contact is four; in other words, we will 
use a friction pyramid in place of a cone. The inverse 
dynamics problem is formulated as follows: 



As discussed in Section |2.3.4[ we have shown that 
the contact model always has a solution (z.e., the QP 
is always feasible) and that the contact forces will not 
do positive work ( [Drumwright fc Shell[ |2Q1Q| ) . The ad¬ 
dition of the inverse dynamics constraint (Equation 
will not change this result—the frictionless version of 
this QP is identical to an LCP of the form that Baraff 
has proven solvable (see Section 4.7), which means that 
the QP is feasible. As in the inverse dynamics approach 
in Section the first order approximation to acceler¬ 
ation avoids inconsistent configurations that can occur 
in rigid contact with Coulomb friction. The worst-case 
time complexity of solving this convex model is polyno¬ 


mial in the number of contact features (Boyd & Van- 


denberghe 2004). High frequency control loops limit n 


to approximately four contacts given present hardware 
and using fast active-set methods. 


6.2.1 Removing equality eonstraints 


The optimization in this section is a convex quadra¬ 
tic program with inequality and equality constraints. 
We remove the equality constraints through substitu¬ 
tion. This reduces the size of the optimization problem; 
removing linear equality constraints also eliminates sig¬ 
nificant variables if transforming the QP to a LCP via 
optimization duality theory (Cottle et al. 1992) ^ 


^ We use such a transformation in our work, which al¬ 
lows us to apply LEMKE ( jFackler fc Miranda] |1997| ) , which is 
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The resulting QP takes the form: 


minimize 

fN,fF 


fN 


NX-^NT NX-ipT' 

./e 

1 

_FX-1nt FX-1ft_ 


-Fk, 



(75) 


subject to: [NX-^N^ NX-^F^] 


/iV 

/e 


— Na^ > 0 


(76) 

fN>0,fF>0 (77) 

fJ'fNi > fPi (78) 


Once fN and fp have been determined, the inverse 
dynamics forces are computed using: 


r+ ■ 
V 

T 


= x-i 


-K + NT/^ + 

^des 


(79) 


As in Section |4.5[ consistency in the desired accelera¬ 
tions can be verified and modified without re-solving 
the QP if found to be inconsistent. 


6.2.2 Minimizing floating point computations 

Because inverse dynamics may be used within real-time 
control loops, this section describes an approach that 
can minimize floating point computations over the for¬ 
mulation described above. 

Assume that we first solve for the joint forces fip 
necessary to solve the inverse dynamics problem under 
no contact constraints. The new velocity v is now de¬ 
fined as: 


+ 

V = 


v+M-\N^fN+F'^fF+Atf,,t+ 


+ X)_ ^ 

(80) 


where we define x to be the actuator forces that are 
added to fip to counteract contact forces. To simplify 
our derivations, we will define the following vectors and 


freely available, numerically robust (using Tikhonov regular¬ 
ization), and relatively fast. 


matrices: 


R = [N^ F^] 

2 = [/JV 



nb nq 

III 

cr 

A B 

nq 

RT C 


nb nq 

. ^ = nb 

D E 

nq 

E^ G 


j =Vh ^ [D E] {Atfext + 

k = Vq [E"'' G] {Atf ext - 


0 

^tfiD 

0 

^tfiD 


) 


fiD = Cq - 


ext,nq 


(81) 

(82) 

(83) 

(84) 

(85) 

( 86 ) 

(87) 


Where nq is the total joint degrees of freedom of the 
robot, and nb is the total base degrees of freedom for 
the robot {nb = 6 for floating bases). 

The components of v are then defined as follows: 


Vb= j + [D E] (Rz + 
\ = k+ [ET G] (Rz ■ 


0 

Atx 


0 

Atx 


) — ^des 


( 88 ) 

(89) 


From the latter equation we can solve for x: 


X = 


G-W-k - [ET G] Rz) 

M 


(90) 


Equation [^indicates that once contact forces are com- 
puted, determining the actuator forces for inverse dy¬ 
namics requires solving only a linear equation. Substi¬ 
tuting the solution for x into Eqn. [ 88 | yields: 

\ = j+[T> E] Rz-hEG-^(\-fc-[ET G] Rz) (91) 


To simplify further derivation, we will define a new ma¬ 
trix and a new vector: 


Z = ([D E] - EG“^ [E^ G]) R (92) 

p = j + EG-W-k) (93) 


Now, can be defined simply, and solely in terms of 
2 :, as: 

= Zz (94) 


We now represent the objective function (Eqn. 69) in 
block form as: 


1 

cr ^ 

T 

A B 


cr 

+ 

. ^9. 


RT C 


1 

Cr 


( 95 ) 
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which is identical to: 

/(•) = 2 A Vq^ -Vq C Vq 


(96) 


As we will be attempting to minimize /(.) with regard 
to 2 :, which the last term of the above equation does not 
depend on, that term is ignored hereafter. Expanding 
remaining terms using Equation [88l the new objective 
function is: 


/(.) = ]^z^7.'^AZz + V, 

= ]^z^T? K'Lz + 3;'^(Z'^Ap + Z'^Bt),) 

subject to the following constraints: 

0 


(97) 

(98) 


N 


Zz+p 

v: 


> -- 


At 


fN,i >0 (for i = 1... n) 

+ CT,i 


Once the solution to this QP is determined, the ac¬ 
tuator forces fID determined via inverse dynamics 

can be recovered. 


After simplifications: Eloating point operations neces¬ 
sary to setup the Stage I model after modified to reduce 
computational costs sum to 73,163 flops when substi¬ 
tuting: m = 18, ng = 16, n = 4, /c = 4, = m — nq^ a 

total of 6.24% reduction in computation. When substi¬ 
tuting: m = 18, ng = 12, n = 4, /c = 4, = m — ng, we 

observed 102,457 flops for this new method and 62,109 
flops before simplification. Thus, a calculation of the 
total number of floating point operations should be per¬ 
formed to determine whether the floating point simpli¬ 
fication is actually more efficient for a particular robot. 



operation 

flops 


(LLT(m))-^ 

-h -h |m 


LL^{G) 

(99) 

Z 

nb m-\- nq n(nk -h l)(2m — 1)-|- 
nb m(2nq — 1) -|- 2nq^m 

P 

2nb nq -h 2nq^ -\- 3nq -\- 2nb -\- 2m -\- 2m nq 

(100) 

Z^AZ 

2nb‘^(n(nk 1)) — nb (n(nk 1)) 


-\-2nb (n(nk -h 1))^ — (n(nk -h 1))^ 

(101) 

Z^Ap 

(n(nk -h 1) + nb)(2nb — 1) 

Z'^Bv* 

(n(nk -h 1) + nq)(2nq — 1) 


N^Z 

n^(nk -h l)(2n6 — 1) 

IP fol- 

nO.';] 

n(2m — 1) 


Table 2: Floating point operations (^fiops^ with floating point op¬ 
timizations. 


6.2.3 Floating point operation counts 


Operation counts for matrix-vector arithmetic and nu¬ 
merical linear algebra are taken from Hunger (2007). 


Before simplifications: Eloating point operations (flops) 
necessary to setup the Stage I model as presented ini¬ 
tially sum to 77,729 flops, substituting: m = 18, nq = 
16, n = 4, /c = 4. 


operation 

flops 

ldlJ{X) 

^ + m^(nq) + + m(nq)^ + 2m(nq) 

+ ^“ 3 ^'+(“<!)" 1 

X-1nT 

m -h 2m^n -\- (nq) -h 4:mn(nq) ^ 2n(nq)‘^ 

X-ipT 

m -h 2km‘^n (nq) 4:kmn(nq) 2kn(nq)^ 

NX-1nT 

2mn‘^ — mn 

fx-^nt 

2mn^k — mn 

FX-ipT 

2mnk‘^ — mnk 

K 

2m^ — m 

X-Ir 

2(m -h (nq))"^ 

NX-^k 

mn — m 

FX-1k 

mnk — m 

T 

2{m -h (nq))^ 


Table 1: Floating point operations (^fiops^ per task without float¬ 
ing point optimizations. 


6 . 2.4 Recomputing Inverse Dynamics to Stabilize 
Actuator Torques (Stage II) 


In the case that the matrix Z^AZ is singular, the con¬ 
tact model is only convex rather than strictly convex 
(Boyd & Vandenberghe 2004). Where a strictly con¬ 
vex system has just one optimal solution, the convex 
problem may have multiple equally optimal solutions. 
Conceptually, contact forces that predict that two legs, 
three legs, or four legs support a walking robot are 
all equally valid. A method is thus needed to optimize 
within the contact model’s solution space while favor¬ 
ing solutions that predict contact forces at all contact¬ 
ing links (and thus preventing the rapid torque cycling 
between arbitrary optimal solutions). As we mentioned 
in Section |2.4[ defining a manner by which actuator 
torques evolve over time, or selecting a preferred dis¬ 
tribution of contact forces may remedy the issues re¬ 
sulting from indeterminacy. One such method would 
select—from the space of optimal solutions to the inde¬ 
terminate contact problem—the one that minimizes the 
^ 2 -norm of joint torques. If we denote the solution that 
was computed in the last section as Zq, the following 
optimization problem will yield the desired result: 
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Fig. 6: Plot of torque chatter while controlling with inverse dynamics using an indeterminate contact model (Stage 1) versus the 
smooth torque profile produced by a determinate contact model (Stage 1 & Stage 2). 



We call the method described in Section [6.2. 2 1 Stage 
I and the optimization problem above Stage 11. Con¬ 
straining for a quadratic objective function (Equation!^ 
with a quadratic inequality constraint yields a QCQP 
that may not be solvable sufficiently quickly for high 
frequency control loops. We now show how to use the 
nullspace of Z^AZ to perform this second optimiza¬ 
tion without explicitly considering the quadratic inequal¬ 
ity constraint] thus, a QP problem formulation is re¬ 
tained. Assume that the matrix W gives the nullspace 
of Z^ AZ. The vector of contact forces will now be given 
as z where w will be the optimization vector. 

The kinetic energy from applying the contact im¬ 
pulses is: 

e2 = ^{z + Wwfz^AZ{z + Ww) 

+ {z + Wwf{Z^Ap + Z^b\) 

= ^z'^Z^AZz + z}<(L^ Ap + Z'^B Vg) 

+ 'uTy^'^^z^Ap + z'^B\q) 

The terms I'uXW'^Z'^AZWw and 2 :Z'''AZWm are 
not included above because both are zero: W is in the 
nullspace of Z^AZ. The energy dissipated in the sec¬ 
ond stage. 62 , should be equal to the energy dissipated 


in the first stage, ei. Thus, we want 62 — 61 = 0. Algebra 
yields: 

'uXw'^{Z'^Ap + Z^B\q) = 0 (104) 


We minimize the ^ 2 -norm of joint torques with respect 
to contact forces by first defining y as: 


y = 


G-i - fe - [E^ G] R(3; + Wit; 

~At 


The resulting objective is: 

9{w) = 


(105) 


(106) 


From this, the following optimization problem arises: 

. 1 T 


mm -y y 
w Z 

.T aT 


subject to: (p A + Vq^'B' )ZWm = 0 


(107) 

(108) 


N' 


Z{z + Ww) + p 




( 109 ) 


{z + Ww)i > 0, (for i = 1... 5n) (110) 

yi{z -1- Wm)i > ^Si{z + Wm)^. -h ... 

^Ti{z + Ww)Ti 

(for i = 1.. .n) ( 111 ) 


Equation |11Q| constrains the contact force vector to 
only positive values, accounting for 5 positive directions 
to which force can be applied at the contact manifold 


(n, s, — 5 , t, — t)p]We use a proof that Z-ker(Z"'"AZ) = 0 

(jZapolsky & Drumwright 

2014 

) to render n + 1 (Equa- 

tions 

tions 

108 

and 109) of 7n + 1 linear constraints (Equa- 

108 

-111) unnecessary. 


^ We consider negative and positive s and i because LEMKE 
requires all variables to be positive. 
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Finally, expanding and simplifying Equation]!^ (re¬ 
moving terms that do not contain in, scaling by 
and using the identity U = [E^ G] R yields: 

g{w) =Iw'^Wm'^G-^^G-^UWw (112) 

+ fe'^G“^^G“^UWw; 

Finally, actuator torques for the robot can be re¬ 
trieved by calculating y ^ f id- 


6.2.5 Feasibility and time eomplexity 

It should be clear that a feasible point {w = 0) always 
exists for the optimization problem. The dimensionality 
(n X n in the number of contact points) of AZ yields 
a nullspace computation of O(n^) and represents one 
third of the Stage II running times in our experiments. 
For quadrupedal robots with single point contacts, for 
example, the dimensionality of w is typically at most 
two, yielding fewer than 6n + 2 total optimization vari¬ 
ables (each linear constraint introduces six KKT dual 
variables for the simplest friction cone approximation). 
Timing results given n contacts for this virtual robot 
are available in Section [8.4.21 


7 Experiments 


This section assesses the inverse dynamics controllers 
under a range of conditions on a virtual, locomoting 
quadrupedal robot (depicted in Figures]^ and a virtual 
manipulator grasping a box. For points of comparison, 
we also provide performance data for three reference 


controllers (depicted in Figures 10 and 12). These ex¬ 
periments also serve to illustrate that the inverse dy¬ 
namics approaches function as expected. 

We explore the effects of possible modeling infideli¬ 
ties and sensing inaccuracies by testing locomotion per¬ 
formance on rigid planar terrain, rigid non-planar ter¬ 
rain, and on compliant planar terrain. The last of these 
is an example of modeling infidelity, as the compliant 
terrain violates the assumption of rigid contact. Sens¬ 
ing inaccuracies may be introduced from physical sensor 
noise or perception error producing, e.g., erroneous fric¬ 
tion or contact normal estimates. All code and data for 
experiments conducted in simulation, as well as videos 
of the virtual robots, are located (and can thus be re¬ 
produced) at: 

http://github.com/PositronicsLab/idyn-experiments. 



7.1 Platforms 

We evaluate performance of all controllers on a sim¬ 
ulated quadruped (see Figure [^. This test platform 
measures 16 cm tall and has three degree of freedom 
legs. Its feet are modeled as spherical collision geome¬ 
tries, creating a a single point contact manifold with 
the terrain. We use this platform to assess the effec¬ 
tiveness of our inverse dynamics implementation with 
one to four points of contact. Results we present from 
this platform are applicable to biped locomotion, the 
only differentiating factor being the presence of a more 
involved planning and balance system driving the quad¬ 
ruped. 



Fig. 8: Snapshot of a quadruped robot in the Moby simulator on 
planar terrain. 


Additionally, we demonstrate the adaptability of this 
approach on a manipulator grasping a box (see Fig¬ 
ure]^. The arm has seven degrees of freedom and each 
finger has one degree of freedom, totaling eleven actu¬ 
ated degrees of freedom. The finger tips have spherical 
collision geometries, creating a a single point contact 
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manifold with a grasped object at each fingertip. The 
grasped box has six non-actuated degrees of freedom 
with a surface friction that is specified in each experi¬ 
ment. 


Fig. 9: Snapshot of a fixed-base manipulator in the Moby simu¬ 
lator grasping a box with four spherieal fingertips. 


swing phase The planned interval of time for a foot to 
be swinging over the ground (to position the foot 
for the next stance phase) 

duty-factor Planned portion of the gait for a foot to be 
in the stance phase 

touchdown Time when a foot makes contact with the 
ground and transitions from swing to stance phase 
liftoff Time when a foot breaks contact with the ground 
and transitions from stance to swing phase 

The trajectories generated by the planner are de¬ 
fined in operational space. Swing phase behavior is cal¬ 
culated as a velocity-clamped cubic spline at the start 
of each step and replanned during that swing phase as 
necessary. Stance foot velocities are determined by the 
desired base velocity and calculated at each controller 
iteration. The phase of the gait for each foot is deter¬ 
mined by a gait timing pattern and gait duty-factor 
assigned to each foot. 

The planner (illustrated in Figure takes as in¬ 
put desired planar base velocity {xb,des = and 

plans touchdown and liftoff locations connected with 
splined trajectories for the feet to drive the robot across 
the environment at the desired velocity. The planner 
then outputs a trajectory for each foot in the local 
frame of the robot. After end effector trajectories have 
been planned, joint trajectories are determined at each 
controller cycle using inverse kinematics. 




7.2 Source of planned trajectories 

7.2.1 Locomotion trajectory planner 

We assess the performance of the reference controllers 
(described below) against the inverse dynamics con¬ 
trollers with contact force prediction presented in this 
work. The quadruped follows trajectories generated by 
our open source legged locomotion software PACEnj^ 
Our software implements balance stabilization, footstep 
planning, spline-based trajectory planning (described 
below), and inverse dynamics-based controllers with si¬ 
multaneous contact force computation that are the fo¬ 
cus of this article. The following terms will be used in 
our description of the planning system: 

gait Cyclic pattern of timings determining when to make 
and break contact with the ground during locomo¬ 
tion. 

stance phase The interval of time where a foot is planned 
to be in contact with the ground (foot applies force 
to move robot) 

^ Pacer is available from: 
http://github.com/PositronicsLab/Pacer 


7.2.2 Arm trajectory planner 

The fixed-base manipulator is command to follow a sim¬ 
ple sinusoidal trajectory parameterized over time. The 
arm oscillates through its periodic motion about three 
times per second. The four fingers gripping the box dur¬ 
ing the experiment are commanded to maintain zero 
velocity and acceleration while gripping the box, and 
to close further if not contacting the grasped object. 


7.3 Evaluated controllers 


We use the same error-feedback in all cases for the pur¬ 
pose of reducing joint tracking error from drift (see 


baseline controller in Figure 10). The gains used for 


PID control are identical between all controllers but dif¬ 
fer between robots. The PID error feedback controller 
is defined in configuration-space on all robots. Balance 
and stabilization are handled in this trajectory plan¬ 
ning stage, balancing the robot as it performs its task. 
The stabilization implementation uses an inverted pen¬ 
dulum model for balance, applying only virtual com¬ 
pressive forces along the contact normal to stabilize 
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the robot (Sugihara & Nakamura 2003). The error- 
feedback and stabilization modules also accumulate error- 
feedback from configuration-space errors into the vector 
of desired acceleration (qdes) input into the inverse dy¬ 
namics controllers. 



We compare the controllers described in this article, 
which will hereafter be referred to as ID (t^) solver,friction, 
where the possible solvers are: solver = {QP, LCP} 
for QP and LCP-based optimization models, respec¬ 
tively; and the possible friction models are: friction = 

{/i, oc} for finite Coulomb friction and no-slip models, 
respectively. 

We compare the controllers implemented using the 
methods described in Sections]^ and [bjsee Figure [IT]) 
against the reference controllers (Figurejl^, using finite 
and infinite friction coefficients to permit comparison 
against no-slip and Coulomb friction models, respec¬ 
tively. Time in ID(tQ refers to the use of contact 
forces predicted at the current controller time. The ex¬ 
perimental (presented) controllers include: ID(tQLCP,oo 
is the ab initio controller from Section |4] that uses an 
LCP model, to predict contact reaction forces with no¬ 
slip constraints; ID (ti) LCP,fi is the ab initio controller 
from Section!^ that uses an LCP model, to predict con¬ 
tact reaction forces with Coulomb friction; lD{ti)Qp^^ 
is the controller from Sectionj^that uses a QP-based op¬ 
timization approach for contact force prediction; ID (tQgp^ 
is the same controller as ID from Section [6^ 

but set to allow infinite frictional forces. 


Fig. 11: Experimental Controllers 
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ID(ti) : Inverse dynamics controller with predictive 
contact forces (this work) generates an estimate 
of contact forces at the current time {z{t)) given 
contact state and internal forces. 



The reference inverse dynamics controllers use sensed 
contact forces; the sensed forces are the exact forces 
applied by the simulator to the robot on the previ¬ 
ous simulation step (z.e., there is a sensing lag of At 
on these measurements, the simulation step size). We 
denote the controller using these exact sensed contact 
forces as ID(ti_i). Controller “ID(ti_i)” uses the ex¬ 
act value of sensed forces from the immediately previous 
time step in simulation and represents an upper limit on 
the performance of using sensors to incorporate sensed 
contact forces into an inverse dynamics model. In situ 
implementation of contact force sensing should result in 
worse performance than the controller described here, 
as it would be subject to inaccuracies in sensing and 
delays of multiple controller cycles as sensor data is fil¬ 
tered to smooth noise; we examine the effect of a second 
controller cycle delay with ID(ti_ 2 ) (see Figure 12). 


^.4 Software and simulation setup 


Pacer runs alongside the open source simulator Mob^ 
which was used to simulate the legged locomotion sce¬ 
narios used in the experiments. Moby was arbitrar¬ 
ily set to simulate contact with the Stewart-Trinkle / 


Anitescu-Potra rigid contact models (Stewart & Trin 


kle 1996 Anitescu & Potra, 1997); therefore, the con¬ 


tact models utilized by the simulator match those used 
in our reference controllers, permitting us to compare 
our contact force predictions directly against those de¬ 
termined by the simulator. Both simulations and con¬ 
trollers had access to identical data: kinematics (joint 


Obtained from https://github.com/PositronicsLab/Moby 
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locations, link lengths, etc.)^ dynamics (generalized in¬ 
ertia matrices, external forces), and friction coefficients 
at points of contact. Moby provides accurate time of 
contact calculation for impacting bodies, yielding more 
accurate contact point and normal information. Other 
simulators step past the moment of contact, and ap¬ 
proximate contact information based on the intersect¬ 
ing geometries. The accurate contact information pro¬ 
vided by Moby allows us to test the inverse dynamics 
controllers under more realistic conditions: contact may 
break or be formed between control loops. 

7.5 Terrain types for locomotion experiments 



Fig. 13: Snapshot of a quadruped robot in the Moby simulator 
on rough terrain. 


We evaluate the performance of the baseline, reference, 
and presented controllers on a planar terrain. We use 
four cases to encompass expected surface properties 
experienced in locomotion. We model sticky and slip¬ 
pery terrain with frictional properties corresponding to 
a Coulomb friction of 0.1 for low (slippery) friction and 
infinity for high (sticky) friction. We also model rigid 
and compliant terrain, using rigid and penalty (spring- 
damper) based contact model, respectively. The com¬ 
pliant terrain is modeled to be relatively hard, yielding 
only 1 mm interpenetration of the foot into the plane 
while the quadruped is at rest. Our contact prediction 
models all assume rigid contact; a compliant terrain will 
assess whether the inverse dynamics model for predic¬ 
tive contact is viable when the contact model of the 
environment does not match its internal model. Suc¬ 
cessful locomotion using such a controller on a com¬ 
pliant surface would indicate (to some confidence) that 
the controller is robust to modeling infidelities and thus 
more robust in an uncertain environment. 

We also test the controllers on a rigid height map 
terrain with non-vertical surface normals and varied 
surface friction to assess robustness on a more natu¬ 
ral terrain. We limited extremes in the variability of 
the terrain, limiting bumps to 3 cm in height (about 


one fifth the height of the quadruped see Figure 13) 


so that the performance of the foothold planner (not 
presented in this work) would not bias performance re¬ 
sults. Friction values were selected between the upper 
and lower limits of Coulomb friction (/i ~ Z//( 0 . 1 ,1.5)) 
found in various literature on legged locomotion. 


locomote successfully: (i) acceleration to and from rest; 
( 2 ) turning in place, and ( 3 ) turning while moving for¬ 
ward. For the trotting gait we assigned gait duration of 
0.3 seconds per cycle, duty-factor 75% of the total gait 
duration, a step height of 1.5 cm, and touchdown times 
{0.25,0.75,0.75,0.25} for {left front, right front, left 
hind, right hind} feet, respectively. These values were 
chosen as a generally functional set of parameters for 
a trotting gait on a 16 cm tall quadruped. The desired 
forward velocity of the robot over the test was 20 cm/s. 
Over the interval of the experiment, the robots are in 
both determinate and possibly indeterminate contact 
configurations and, as noted above, undergo numerous 
contact transitions. We show (in Section 8.0. 1|) that the 


controllers we present in Sect ions and |4 are feasible for 
controlling a quadruped robot over a trot; we compare 
contact force predictions made by all presented control¬ 
lers to the reaction forces generated by the simulation 


(Section 8 . 2 ); and we measure running times of all pre¬ 
sented controllers given numerous additional contacts 
in Section 18.4.21 

We simulate the manipulator grasping a box while 
following a simple, sinusoidal joint trajectory. During 
this process the hand is susceptible to making con¬ 
tact transitions as the box slips from the grasp. We 
record the divergence from the desired trajectory over 
the course of the experiments. We note that the ob¬ 
jective of this task is to accurately follow the joint 
trajectory—predicting joint torques and contact forces 
with rigid contact constraints—not to hold onto the box 
firmly. 


7.6 Tasks 

We simulate the quadruped trotting between a set of 
waypoints on a planar surface for 30 virtual seconds. 
The process of trotting to a waypoint and turning to¬ 
ward a new goal stresses some basic abilities needed to 


8 Results 

This section quantifies and plots results from the ex¬ 
periments in the previous section in five ways: ( 1 ) joint 
trajectory tracking; ( 2 ) accuracy of contact force pre- 
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diction; ( 3 ) torque command smoothness; ( 4 ) center-of- 
mass behavior over a gait; ( 5 ) computation speed. 


8.0.1 Trajectory tracking on planar surfaces: 


We analyze tracking performance using the quadruped 
platform on both rigid planar and compliant planar 
surfaces (see Figure 14). Joint tracking data was col¬ 
lected from the simulated quadruped using the base¬ 
line, reference and experimental controllers to locomote 
on a planar terrain with varying surface properties. 
Numerical results for these experiments are presented 
in Table The experimental controllers implementing 
contact force prediction,ID(ti), either outperformed or 
matched the performance of the inverse dynamics for¬ 
mulations using sensed contact, ID(t^_i) and ID(t^_ 2 ). 


As expected, the baseline controller (PID) performed 
substantially worse than all inverse dynamics systems 
for positional tracking on a low friction surface. Also, 
only the inverse dynamics controllers that use predic¬ 
tive contact managed to gracefully locomote with no¬ 
slip contact. 


The reference inverse dynamics controllers with sensed 
contact performed the worst on high friction surfaces, 
only serving to degrade locomotion performance from 
the baseline controller over the course of the experi¬ 
ment. We assume that the performance of a well tuned 
PID error-feedback controller may be due to the control 
system absorbing some of the error introduced by poor 
planning, where more accurate tracking of planned tra¬ 
jectories may lead to worse overall locomotion stability. 


Trajectory Tracking: Quadruped 


High friction (fi = 00), rigid surface 



Low friction (/x = 0.1), rigid surface 



High friction (/x = 00), compliant surface 



Low friction (/j, = 0.1), compliant surface 



Fig. 14: Average position error for all joints (E[\6 — over 

time while the quadruped performs a trotting gait. 
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8.1 Smoothness of torque commands 


Torque Chatter 



Fig. 15: Time derivative torque when using the inverse dynamics 
method (ID(ti)Qp^^) with means for mitigating torque chatter 
from indeterminate contact (red/dotted) vs. no such approach 
(black/solid). 


Torque Smoothness 


Controller 

E[\At\] 



52.5718 

0.2016 

ID(C)qp,oo 

245.5117 

0.4239 

TD{ti)LCP,^ 

677.9693 

0.8351 

ID(C)lC'P,oo 

351.1803 

0.4055 

ID(t,_i) 

974.3312 

0.9918 

ID(C_2) 

17528.0 

2.7416 

PID 

100.1571 

0.3413 


Table 4: Average derivative torque magnitude (denoted E'[|zAr|]^ 
and average torque magnitude (denoted E[|t|]^ for all controllers. 

Figure shows the effects of an indeterminate con¬ 
tact model on torque smoothness. Derivative of torque 
commands are substantially smaller when incorporating 
a method of mitigating chatter in the inverse dynamics- 
derived joint torques. We observe a five order of magni¬ 
tude reduction in the maximum of the time derivative 
torque when using a torque smoothing stage with the 
Drumwright-Shell contact model. Controller ID(t^) lcp,/x 
is the only presented controller unable to mitigate torque 
chatter (seen in the “indeterminate” case in Figure [T^ 
and therefore produces the worst performance from the 
presented inverse dynamics methods. Though it demon¬ 
strates successful behavior in simulation, this method 
would likely not be suitable for use on a physical plat¬ 
form. The reference inverse dynamics controllers (ID(t4_i) 
and ID(t^_2)) exhibit significant torque chatter also. 

We measure the “smoothness” of torque commands 
as the mean magnitude of derivative torque over time. 
We gather from the data presented in Table that 
the two phase QP-based inverse dynamics controller 


(ID(ti)Qp^^) followed by the baseline controller (PID) 
are the most suitable for use on a physical platform. 
Controller uses the lowest torque to loco- 

mote while also mitigating sudden changes in torque 
that may damage robotic hardware. 


8.2 Verification of correctness of inverse dynamics 

We verify correctness of the inverse dynamics approaches 
by comparing the contact predictions against the reac¬ 
tion forces generated by the simulation. The compari¬ 
son considers only the ^i-norm of normal forces, though 
frictional forces are coupled to the normal forces (so ig¬ 
noring the frictional forces is not likely to skew the re¬ 
sults). We evaluate each experimental controller’s con¬ 
tact force prediction accuracy given sticky and slippery 
frictional properties on rigid and compliant surfaces. 

The QP-based controllers are able to predict the 
contact normal force in simulation to a relative error 
between 12-30% j^The ID(ti)LCPp, ID(^i)LCP,oo con¬ 
trollers demonstrated contact force prediction between 
1.16-1.94% relative error while predicting normal forces 
on a rigid surface (see Table [^. The QP based control¬ 
lers performed as well on a compliant surface as they 
did on the rigid surfaces, while the performance of the 
IE)(tQLCP,/i5 IE)(tQLCP,/x controllers was substantially 
degraded on compliant surfaces. 

The LCP-based inverse dynamics models (ID(tQ LCP,g 
and ID (ti) LCP,oo) CISC a contact model that matches 
that used by the simulator. Nevertheless no inverse dy¬ 
namics predictions always match the measurements pro¬ 
vided by the simulator. Investigation determined that 
the slight differences are due to (i) occasional inconsis¬ 
tency in the desired accelerations (we do not use the 
check described in Section [T^ ; (2) the approximation 
of the friction cone by a friction pyramid in our ex¬ 
periments (the axes of the pyramid do not necessarily 
align between the simulation and the inverse dynamics 
model); and (3) the regularization occasionally neces¬ 
sary to solve the LCP (inverse dynamics might require 
regularization while the simulation might not, or vice 
versa). 


8.3 Controller behavior 

The presented data supports the utilization of the QP- 
based inverse dynamics model incorporating Coulomb 

^ The QP-based inverse dynamics models use a contact 
model that differs from the model used within the simula¬ 
tor. When the simulation uses the identical QP-based contact 
model, prediction exhibits approximately 1% relative error. 
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Trajectory Tracking Error 


Rigid, Low Friction 

Controller 

positional error 

velocity error 

ID(C)QP,At 

0.0310 

1.9425 

ID(C)qp,oo 

0.0483 

2.6295 

l'D{ti)LCP,^^ 

0.0239 

1.8386 

ID(C)lCP,oo 

- 

- 

PID 

0.0895 

1.5569 

ID(C_i) 

0.0325 

1.7952 

lD(q 2) 

0.0328 

1.7830 

Compliant, Low Friction 

Controller 

positional error 

velocity error 

ID(C)qp,m 

0.0217 

2.0365 

ID(C)qp,oo 

- 

- 

ID(C)pcp,Ai 

0.0219 

2.0786 

ID(ti)pcp,oo 

- 

- 

PID 

0.0850 

1.5845 

ID(C_i) 

0.0265 

1.8858 

ID(C_2) 

0.0267 

1.8742 


Rigid, High Friction 

Controller 

positional error 

velocity error 

ID(C)QP,At 

0.0486 

2.2596 

ID(C)qp,oo 

0.0654 

2.5737 

l'D{ti)LCP,n 

0.0259 

1.8784 

I'D{ti)LCP,oo 

0.0260 

1.8950 

PID 

0.0916 

1.4653 

ID(C_i) 

0.1317 

2.5585 

ID(C 2) 

0.1316 

2.5608 

Compliant, High Friction 

Controller 

positional error 

velocity error 

ID(C)qp,m 

0.0342 

2.9360 

ID(C)qp,oo 

0.0446 

3.9779 

ID(C)lcp,ax 

0.0226 

2.1243 

ID(C)lcp,oo 

- 

- 

PID 

0.0850 

1.5845 

ID(C_i) 

0.1270 

4.4377 

ID(C_2) 

0.1270 

4.2061 


Table 3: Expected trajectory tracking error for quadrupedal locomotion (positional: mean magnitude of radian error for all joints 
over trajectory duration (E[E[\0 — velocity: mean magnitude of radians/second error for all joints over trajectory duration 

(E[E[\6 — Oy^QsW])) of inverse dynamics controllers (ID(..)) and baseline (PID) controller. 

Contact Force Prediction Error 


Rigid, Low Friction 

Controller 

absolute error 

relative error 

ID(C)qp,m 

3.8009 N 

12.53% 

ID(C)qp,oo 

8.4567 N 

22.26% 

I'D{ti)LCP,ix 

0.9371 N 

1.94% 

ID(C)lcp,oo 

- 

- 

Compliant, Low Friction 

Controller 

absolute error 

relative error 

ID(C)qp,m 

7.8260 N 

17.12% 

ID(C)qp,oo 

- 

- 

I'D{ti)LCP,ix 

4.6385 N 

6.37% 

ID(C)lcp,oo 

- 

- 


Rigid, High Friction 

Controller 

absolute error 

relative error 

ID(tDQP,M 

13.8457 N 

27.48% 

ID(C)qp,oo 

12.4153 N 

25.26% 

ID(C)lcp,ax 

1.2768 N 

1.55% 

l'D{ti)LCP,oo 

0.3572 N 

1.16 % 

Compliant, High Friction 

Controller 

absolute error 

relative error 

ID(C)qp,m 

14.9225 N 

30.86% 

ID(C)qp,oo 

15.1897 N 

30.66% 

I'D{ti)LCP,^^ 

12.4896 N 

24.00% 

ID(C)lcp,oo 

- 

- 


Table 5: Average contact force prediction error (summed normal forces) of inverse dynamics controllers vs. measured reaction forces 
from simulation. The quadruped exerts 4.7.0882 N of force against the ground when at rest under standard gravity. Results marked 
with a indicate that the quadruped was unable to complete the locomotion task before falling. 


friction, at least for purposes of control of existing phys¬ 
ical hardware. We also observed that utilizing Coulomb 
friction in inverse dynamics leads to much more stable 
locomotion control on various friction surfaces. The no¬ 
slip contact models proved to be more prone to pre¬ 
dicting excessive tangential forces and destabilizing the 
quadruped while not offering much additional perfor¬ 
mance for trajectory tracking. Accordingly, subsequent 
results for locomotion on a height map and control¬ 
ling a fixed-base manipulator while grasping a box are 
reported only for ID which can be referred to 

more generally as “the inverse dynamics controller with 
contact force prediction” or ID(t 4 ). 

Rigid non-planar surface: Figure plots trajectory 
tracking performance of the locomoting quadruped on 
rigid terrain with variable friction (ranging between low 
and high values of Coulomb friction for contacting ma¬ 


terials as reported in literature). Three reference con¬ 
trollers are compared against our inverse dynamics con¬ 
troller. During this experiment only the ideal sensor 
controller ID(t^_i) consistently produced better posi¬ 
tional tracking than our proposed controller 
Our experimental controller reduced tracking error be¬ 
low that of error-feedback control alone by 19%. 
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Trajectory Tracking: Quadruped 


Random friction, rigid heightmap 



Fig. 16: Joint trajectory tracking for a quadruped on a rigid 
heightmap with uniform random friction pi ~ W(0.1,1.5). 

8.4 Center-of-mass tracking performance 

The ability of the controller to track the quadruped’s 
center-of-mass over a path is a met a metric, as one ex¬ 
pects this metric to be dependent upon joint tracking 
accuracy. Figure shows that both the PID and the 
ID(ti) methods are able to track straight line paths 
fairly well. IDCi-i), which yielded better joint posi¬ 
tion tracking, does not track the center-of-mass as well. 
ID(ti_2) results in worse tracking with respect to both 
joint position and center-of-mass position. We hypoth¬ 
esize that this discrepancy is due to our observation 
that IDCi-i) and ID(t^_ 2 ) yield significantly larger 
joint velocity tracking errors than the PID and ID(ti) 
controllers. 

8.4-1 Fixed base manipulator grasping a box 

Trajectory tracking results for the fixed-base manipula¬ 
tor are presented in Figure We observed large errors 
in the PID and ID(ti_i) controllers while grasping 
the box, the sensed force inverse dynamics controller 
was adversely affected when attempting to manipulate 
the sticky object, applying excessive forces while ma¬ 
nipulating the box with high friction {ja = oc). Both 
the PID and ID{ti)Qp^^ controllers dropped the box 
with low friction (/i = 1.0) at about 1500 milliseconds. 
We observed the trajectory error quickly converge to 
zero after the inverse dynamics method dropped the 
grasped object, while the PID controller maintained a 
fairly high level of positional error. The sensed contact 
inverse dynamics controller ID(t^_i) performed at the 
same accuracy as the predictive contact force inverse 
dynamics controller, and managed to not drop the box 
over the course of the three second experiment. 


Though the box slipped from the grasp of the inverse 
dynamics controlled manipulator, its tracking error did 
not increase substantially. This demonstrates a capa¬ 
bility of the controller to direct the robot through the 
task with intermittent contact transitions with heavy 
objects, while maintaining accuracy in performing its 
trajectory-following task. 


Trajectory Tracking: Manipulator 
High friction (/a = oo), rigid surface 




Time (ms) 

Fig. 18: Joint trajectory tracking for a fixed base manipulator 
grasping a heavy box (6000 with friction: (top) pi = oo — 

no-slip; and (bottom) p = 1. 
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Fig. 17: Center-of-mass path in the horizontal plane between waypoints over 30 seeonds. (left) high frietion; (right) low frietion. The 
quadruped is commanded to follow straight line paths between points {(0, 0), (0.25, 0), (0, 0.25), (0, —0.25), (—0.25, 0)}. 


8.4-2 Running time experiments 



0 5 10 15 20 25 30 35 40 

Number of contacts 


Controller duration vs. Contacts 


Fig. 19: Inverse dynamics controller runtimes for increasing 
numbers of contacts (Quadruped with spherical feet). 


We measured the computation time for each controller 
during the quadruped experiments, artificially increas¬ 
ing the number of simultaneous contacts at each foot- 
ground interface p] Fignre shows that inverse dynam¬ 

ics method ID{ti)LCP,oo scales linearly with additional 
contacts. The fast pivoting algorithm {ID{ti)LCP,oo) 
can process in excess of 40 contacts while maintaining 
below a 1 ms runtime—capable of 1000 Hz control rate. 
The experimental QP-based controllers: ID 005 
ID (ti)QPp supported a control rate of 

® Experiments were performed on a 2011 MacBook Pro 
Laptop with a 2.7 GHz Intel Core i7 CPU. 


1000 Hz to about 30 total contacts when warm-starting 
the LCP solver with the previous solution. Control¬ 
ler ID(ti)pc'P,Ai did not support warm-starting or the 
fast pivoting algorithm and was only able to maintain 
around a 1 ms expected runtime for fewer than 4 con¬ 
tacts. The runtime for ID(ti)pc'P,oo was substantially 
higher than the QP-based model, despite a significantly 
reduced problem size, for fewer than approximately 30 
points of contact. This disparity is due to the high com¬ 
putational cost of Lines 6 and 10 in Algorithmic 


8.5 Discussion of inverse dynamics based control for 
legged locomotion 


The inverse dynamics controller that predicts contact 
forces (ID(tQQPp) performs well, at least in simula¬ 
tion, while mitigating destructive torque chatter. Over 
all tests, we observe the use of inverse dynamics con¬ 
trol with predicted contact forces, ie., ID(tQ, can more 
closely track a joint trajectory than the alternatives— 
including inverse dynamics methods using sensed con¬ 
tact forces (even with perfect sensing) and PID control. 
The inverse dynamics controller described in this work 
ID(tQ and the baseline PID controller, were able to 
track center-of-mass position of quadrupeds with little 
deviation from the desired direction of motion, while 
the inverse dynamics controllers using sensed contact 
forces performed substantially worse at this task (as 
seen in Section 8.4). With these results in mind we 
find that contact force prediction has more potential 
for producing successful robot locomotion on terrain 
with varied surface properties. We expect that as sensor 
technology and computational speeds improve, contact 







































30 


force prediction for inverse dynamics control will even 
more greatly outperform methods dependent on force 
sensors. 


9 Conclusion 

We presented multiple, fast inverse dynamics methods— 
a method that assumes no slip (extremely fast), a QP- 
based method without complementarity (very fast), that 
same method with torque chattering mitigation (fast 
enough for real-time control loops at lOOOHz using cur¬ 
rent computational hardware on typical quadrupedal 
robots), and an LCP-based method that enforces com¬ 
plementarity (fast). We showed that a right inverse ex¬ 
ists for the rigid contact model with complementarity, 
and we conducted asymptotic time complexity analysis 
for all inverse dynamics methods. Each method is likely 
well suited to a particular application. For example. 
Section found that the last of these methods yields 
accurate contact force predictions (and therefore better 
joint trajectory tracking) for virtual robots simulated 
with the Stewart-Trinkle/Anitescu-Potra contact mod¬ 
els. Finally, we assessed performance—running times, 
joint space trajectory tracking accuracy, and an indica¬ 
tion of task execution capability (for legged locomotion 
and manipulation)—under various contact modeling as¬ 
sumptions. 
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The table in this section (Table [^ scores each inverse dynamics controller implementation according to its qualitative performance over the course of 
each experiment. 
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D The Principal Pivoting Method for solving 
LCPs 


B Generalized contact wrenches 


A contact wrench applied to a rigid body will take the 
form: 

r X q 

where g is a vector in and r is the vector from the 
center of mass of the rigid body to the point of contact 
(which we denote p). For a multi-rigid body defined in 
m minimal coordinates, a generalized eontaet wreneh 
Q G for single point of contact p would take the 
form: 


(113) 


Q = jTq 

where J G 


(114) 

is the manipulator Jacobian (see, 


e.g.. 
to p. 


Sciavicco & Siciliano 2000 ) computed with respect 


C Relationship between LCPs and MLCPs 


This section describes the mixed linear eomplimentar- 
ity problem (MLCP) and its relationship to the “pure” 
LCR 

Assume the LCP (r, Q) for r G and Q G 

w = Qz-\^r w>0 z>0 z^w = 0 ( 115 ) 

for unknown vectors 2 :, m G A mixed linear comple¬ 
mentarity problem (MLCP) is defined by the matrices 
A G C G D G B G x G M", 

y G g G W, and h e W (where p = s and r = t) 
and is subject to the following constraints: 


Ax + C 2 : + gf = 0 

(116) 

Bx + Bz h >0 

(117) 

z>0 

(118) 

z^{Bx + B 2 : + /i) = 0 

(119) 


The X variables are unconstrained, while the 2 : vari¬ 
ables must be non-negative. If A is non-singular, the 
unconstrained variables can be computed as: 

X = -A-^(C;2 + ^) (120) 


Substituting x into Equations |116f|119| yields the pure 
LCP (r,Q): 


Q = B-DA-^C (121) 

r = h- BA-^g (122) 


A solution ( 2 :, w) to this LCP obeys the relationship 
Qz-]-r = W] given 2 :, cc is determined via Equation |120[ 
solving the MCLP. 


The Principal Pivot Method I (Cottle 1968| [Mnrty 
1988[ ) (PPM), which solves LCPs with P-matrices (com¬ 
plex square matrices with fully non-negative principal 
minors (Mnrty 1988[ ) that includes positive semi-definite 
matrices as a proper subset). The resulting algorithm 
limits the size of matrix solves and multiplications. 

The PPM uses sets a, a, /3, and p for LCP variables 
2 : and w. The first two sets correspond to the 2 : variables 
while the latter two correspond to the w variables. The 
sets have the following properties for an LCP of order 


n\ 


1. (a U (a = {1,..., n} 

2. (a n a = 0 

3. /3U^ = {l,...,n} 

4 . = 0 

Of a pair of LCP variables, {zi^Wi)^ index i will either 
be in a and (3 oi ^ and a. If an index belongs to a 
or /3, the variable is a basie variable] otherwise, it is 
a non-basie variable. Using this set, partition the LCP 
matrices and vectors as shown below: 


'W0 


-^f3a 

-^(3a 


Za 

+ 

9 / 3 ' 






_Za_ 

Sip. 


Isolating the basic and non-basic variables on different 
sides yields: 









^i3a 

A- A“^ 





_Wg_ 


-A -1 








If we set the values of the basic variables to zero, then 
solving for the values of the non-basic variables z-^ and 
w-^ entails only computing the vector (repeated from 
above): 




(123) 


PPM I operates in the following manner: ( 1 ) Eind an 
index i of a basic variable Xi (where Xi is either Wi or Zi , 
depending which of the two is basic) such that < 0 ; 
( 2 ) swap the variables between basic and non-basic sets 
for index i (e.^., liwi is basic and Zi is non-basic, make 
Wi non-basic and Zi basic); ( 3 ) determine new values of 
2 : and W] ( 4 ) repeat ( 1 ) -( 3 ) until no basic variable has 
a negative value. 
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E Proof that removing linearly dependent Then the LCP vectors q = Nv, 2 : G and w G 

equality constraints from the MLCP in and LCP matrix Q = NM“^N^ can be partitioned as 

Section 14.41 does not alter the MLCP solution follows: 


Theorem 1 The solution to the MLCP in Equations\4^ 
and without linearly dependent equality eonstraints 
removed from A is identieal to the solution to the MLCP 
with redueed A matrix and uneonstrained variables set 
to zero that eorrespond to the linearly dependent equal¬ 
ity eonstraints. 


Proof Assume that U is a matrix with rows consisting 
of a set of linearly independent vectors {ui,... 
where n G N. Each of these vectors comes from a row 
of P, S, or T. Assume W is a matrix with rows consist¬ 
ing of vectors {wi ,..., each of which is a linear 

combination of the rows of U, for m G N. U and W 
are related in the following way: Z • U = W, for some 
matrix Z. The MLCP from Equations and ^ can 
then be rewritten as: 


’M -W-(ZU)"^-NT' 
U 0 0 0 

ZU 0 0 0 

NO 0 0 


/jV > 0, Wiv > 0, fJjWN = 0 


- 

r + n 
V 


K 


" 0 " 


fu 

0 

+ 

0 

0 

= 

0 

0 

- 

Jn_ 


_1 


_Wn_ 


(124) 

(125) 


where fu are unconstrained variables that correspond 
to the linearly independent equality constraints. Note 
that the value of 0 is assigned to the variables corre¬ 
sponding to the linearly dependent equality constraints. 
Since values for fu, and fN that satisfy the equa¬ 
tions above require U v = 0, the constraint ZU v = 0 
is automatically satisfied. □ 


F Proof that no more than m positive force 
magnitudes need be applied along contact 
normals to a m degree of freedom multibody 
to solve contact model constraints 


This proof will use the matrix of generalized contact 
wrenches, N G (introduced in Section 4.1), and 

M G the generalized inertia matrix for the 

multi-body. Zj is the vector of contact force magnitudes 
and consists of strictly positive values. 

Assume we permute and partition the rows of N 
into r linearly independent and n — r linearly depen¬ 
dent rows, denoted by indices I and respectively, as 
follows: 


N = 


N/ 

N,, 


(126) 


Qii Qid 

zi 

1 

Qi 

_Qdi Qdd_ 





Wi 

Wd 


(127) 


Given some matrix 7 G it is the case that 

Nd = 7 N/, and therefore that Qdi = 7 N/M“^N/"'", 
Qid = N/M“^N/’^ 7 ’^ (by symmetry), 

and qd = 7 N/t;. 


Lemma 1 Sinee rankCNM.) < min (ranA:(N), ranA;(M)), 
the number of positive eomponents of Zj ean not be 
greater than ranA;(N). 

Proof The columns of NM have N multiplied by each 
column of M, z.e., NM = [Nmi Nm 2 ... Nm^]. Col¬ 
umns in M that are linearly dependent will thus pro¬ 
duce columns in NM that are linearly dependent (with 
precisely the same coefficients). Thus, rank(NM) < 
rank(M). Applying the same argument to the trans¬ 
poses produces rank(NM) < rank(N), thereby proving 
the claim. □ 

We now show that no more positive force magnitudes 
are necessary to solve the LCP in the case that the 
number of positive components of Zj is equal to the 
rank of N. 

Theorem 2 If {zj = a, wj = 0) is a solution to the 
LCP {qi, Qii), then {[zi^ = of Zd^ = O"'"]"'", = 0) 

is a solution to the LCP (gr, Q). 

Proof Eor ( [zi~^ = zd^ = 0^]^ , it? = 0 ) to be a so¬ 
lution to the LCP (gr, Q), six conditions must be satis¬ 
fied: 

1 . zi>0 

2 . wi>0 

3 . zj^wj = 0 

4 . zd>0 

5. Wd P 0 

6 . zd^wd = 0 

Of these, ( 1 ), ( 4 ), and ( 6 ) are met trivially by the 
assumptions of the theorem. Since zd = 0^ QiiZj + 
QidZd P Qi = and thus Wj = 0 , thus satisfying ( 2 ) 
and ( 3 ). Also due to Zd = P suffices to show for ( 5 ) 
that ClDi^iPqD P 0- Erom above, the left hand side of 
this equation is equivalent to 7 (N/M“^N/^a + N/v), 
or 7 it?/, which itself is equivalent to 7 O. Thus, wd = 

□ 
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